Merge pull request #41 from JuliaControl/kalman

Kalman

Author: mfalt

src/ControlSystems.jl

@@ -15,6 +15,8 @@ export  LTISystem,
         dlyap,
         lqr,
         dlqr,
+        kalman,
+        dkalman,
         covar,
         norm,
         gram,
@@ -54,7 +56,10 @@ export  LTISystem,
         evalfr,
         bode,
         nyquist,
-        sigma
+        sigma,
+        # utilities
+        numpoly,
+        denpoly
 
 import Plots
 import Base: +, -, *, /, (./), (==), (.+), (.-), (.*), (!=), isapprox, call, convert

src/synthesis.jl

@@ -17,6 +17,13 @@ function lqr(A, B, Q, R)
     return K
 end
 
+@doc """`kalman(A, C, R1, R2)` kalman(sys, R1, R2)`
+
+Calculate the optimal Kalman gain
+
+""" ->
+kalman(A, C, R1,R2) = lqr(A',C',R1,R2)'
+
 function lqr(sys::StateSpace, Q, R)
     if iscontinuous(sys)
         return lqr(sys.A, sys.B, Q, R)
@@ -25,6 +32,14 @@ function lqr(sys::StateSpace, Q, R)
     end
 end
 
+function kalman(sys::StateSpace, R1,R2)
+    if iscontinuous(sys)
+        return lqr(sys.A', sys.C', R1,R2)'
+    else
+        return dlqr(sys.A', sys.C', R1,R2)'
+    end
+end
+
 @doc """`dlqr(A, B, Q, R)`, `dlqr(sys, Q, R)`
 
 Calculate the optimal gain matrix `K` for the state-feedback law `u[k] = K*x[k]` that
@@ -39,6 +54,13 @@ function dlqr(A, B, Q, R)
     return K
 end
 
+@doc """`dkalman(A, C, R1, R2)` kalman(sys, R1, R2)`
+
+Calculate the optimal Kalman gain for discrete time systems
+
+""" ->
+dkalman(A, C, R1,R2) = dlqr(A',C',R1,R2)'
+
 @doc """`place(A, B, p)`, `place(sys::StateSpace, p)`
 
 Calculate gain matrix `K` such that
@@ -83,14 +105,40 @@ feedback(L::TransferFunction) = L/(1+L)
 feedback(P::TransferFunction, C::TransferFunction) = feedback(P*C)
 
 
+"""
+`feedback(sys)`
+
+`feedback(sys1,sys2)`
+
+Forms the negative feedback interconnection
+```julia
+>-+ sys1 +-->
+  |      |
+ (-)sys2 +
+```
+If no second system is given, negative identity feedback is assumed
+"""
+function feedback(sys::StateSpace)
+    sys.ny != sys.nu && error("Use feedback(sys1::StateSpace,sys2::StateSpace) if sys.ny != sys.nu")
+    feedback(sys,ss(eye(sys.ny)))
+end
+
+function feedback(sys1::StateSpace,sys2::StateSpace)
+    sum(abs(sys1.D)) != 0 && sum(abs(sys2.D)) != 0 && error("There can not be a direct term (D) in both sys1 and sys2")
+    A = [sys1.A+sys1.B*(-sys2.D)*sys1.C sys1.B*(-sys2.C); sys2.B*sys1.C  sys2.A+sys2.B*sys1.D*(-sys2.C)]
+    B = [sys1.B; sys2.B*sys1.D]
+    C = [sys1.C  sys1.D*(-sys2.C)]
+    ss(A,B,C,sys1.D)
+end
+
+
 """
 `feedback2dof(P,R,S,T)` Return `BT/(AR+ST)` where B and A are the numerator and denomenator polynomials of `P` respectively
 `feedback2dof(B,A,R,S,T)` Return `BT/(AR+ST)`
 """
 function feedback2dof(P::TransferFunction,R,S,T)
-    if !issiso(P)
-        error("Feedback not implemented for MIMO systems")
-    end
-     tf(conv(poly2vec(numpoly(P)[1]),T),zpconv(poly2vec(denpoly(P)[1]),R,poly2vec(numpoly(P)[1]),S))
+    !issiso(P) && error("Feedback not implemented for MIMO systems")
+    tf(conv(poly2vec(numpoly(P)[1]),T),zpconv(poly2vec(denpoly(P)[1]),R,poly2vec(numpoly(P)[1]),S))
  end
+ 
 feedback2dof(B,A,R,S,T) = tf(conv(B,T),zpconv(A,R,B,S))

src/types/polys.jl

@@ -178,11 +178,11 @@ function ==(p1::Poly, p2::Poly)
     end
 end
 
-function isapprox(p1::Poly, p2::Poly)
+function isapprox(p1::Poly, p2::Poly; kwargs...)
     if length(p1) != length(p2)
         return false
     else
-        return p1.a[p1.nzfirst:end] ≈ p2.a[p2.nzfirst:end]
+        return isapprox(p1.a[p1.nzfirst:end], p2.a[p2.nzfirst:end]; kwargs...)
     end
 end
 

src/types/sisogeneralized.jl

@@ -88,7 +88,7 @@ SisoRational(expr::Number) = isa(expr,Real) ? SisoRational([expr],[1]) : error("
 
 ==(t1::SisoGeneralized, t2::SisoGeneralized) = (t1.expr == t2.expr)
 
-#isapprox(t1::SisoRational, t2::SisoRational) = (t1.num ≈ t2.num && t1.den ≈ t2.den)
+isapprox(t1::SisoGeneralized, t2::SisoGeneralized; kwargs...) = error("isapprox not implemented for generalized tf")
 
 +(t1::SisoGeneralized, t2::SisoGeneralized) = SisoGeneralized(:($(t1.expr) + $(t2.expr)))
 +(t::SisoGeneralized, n::Real) = SisoGeneralized(:($(t.expr) + $n))

src/types/sisotf.jl

@@ -85,7 +85,9 @@ pole(t::SisoRational) = roots(t.den)
 
 ==(t1::SisoRational, t2::SisoRational) = (t1.num == t2.num && t1.den == t2.den)
 # We might want to consider alowing scaled num and den as equal
-isapprox(t1::SisoRational, t2::SisoRational) = (t1.num ≈ t2.num && t1.den ≈ t2.den)
+function isapprox(t1::SisoRational, t2::SisoRational; rtol::Real=sqrt(eps()), atol::Real=sqrt(eps()))
+    isapprox(t1.num,t2.num, rtol=rtol, atol=atol) && isapprox(t1.den, t2.den, rtol=rtol, atol=atol)
+end
 
 +(t1::SisoRational, t2::SisoRational) = SisoRational(t1.num*t2.den + t2.num*t1.den, t1.den*t2.den)
 +(t::SisoRational, n::Real) = SisoRational(t.num + n*t.den, t.den)

src/types/sisozpk.jl

@@ -150,9 +150,9 @@ Base.length(t::SisoZpk) = max(length(t.z), length(t.p))
 
 
 ==(t1::SisoZpk, t2::SisoZpk) = (t1-t2).k == 0.0
-function isapprox(t1::SisoZpk, t2::SisoZpk, res = sqrt(eps()))
+function isapprox(t1::SisoZpk, t2::SisoZpk; rtol::Real=sqrt(eps()), atol::Real=sqrt(eps()))
     tdiff = t1 - t2
-    isapprox(tdiff.k, 0, atol=res)
+    isapprox(tdiff.k, 0, atol=atol, rtol=rtol)
 end
 
 function +(t1::SisoZpk, t2::SisoZpk)

src/types/tf2ss.jl

@@ -104,18 +104,28 @@ function balance_transform{R}(A::Matrix{R}, B::Matrix{R}, C::Matrix{R}, perm::Bo
     return T
 end
 
-@doc """`sys = ss2tf(s::StateSpace)`, ` sys = ss2tf(A, B, C, Ts = 0, inames = "", onames = "")`
+@doc """`sys = ss2tf(s::StateSpace)`, ` sys = ss2tf(A, B, C, Ts = 0; inputnames = "", outputnames = "")`
 
 Convert a `StateSpace` realization to a `TransferFunction`""" ->
 function ss2tf(s::StateSpace)
-    return ss2tf(s.A, s.B, s.C, s.Ts, s.inputnames, s.outputnames)
+    return ss2tf(s.A, s.B, s.C, s.Ts, inputnames=s.inputnames, outputnames=s.outputnames)
 end
 
-function ss2tf(A, B, C, Ts = 0, inames = "", onames = "")
+function ss2tf(A, B, C, Ts = 0; inputnames = "", outputnames = "")
+    nu,ny = size(B,2),size(C,1)
+    ubernum = Matrix{Vector}(ny,nu)
+    uberden = Matrix{Vector}(ny,nu)
+    for i = 1:nu, j=1:ny
+        ubernum[j,i],uberden[j,i] = sisoss2tf(A, B[:,i], C[j,:])
+    end
+    tf(ubernum,uberden, Ts, inputnames=inputnames, outputnames=outputnames)
+end
+
+function sisoss2tf(A, B, C)
     charpolA = charpoly(A)
     numP = charpoly(A-B*C) - charpolA
     denP = charpolA
-    return tf(numP[1:length(numP)], denP[1:length(denP)], Ts, inputnames=inames, outputnames=onames)
+    return numP[1:length(numP)], denP[1:length(denP)]
 end
 
 tf(sys::StateSpace) = ss2tf(sys)
@@ -124,7 +134,7 @@ zpk(sys::StateSpace) = zpk(ss2tf(sys))
 function charpoly(A)
     λ = eigvals(A);
     p = reduce(*,ControlSystems.Poly([1.]), ControlSystems.Poly[ControlSystems.Poly([1, -λᵢ]) for λᵢ in λ]);
-    if maximum(imag(p[:])) < sqrt(eps())
+    if maximum(imag(p[:])./(1+abs(real(p[:])))) < sqrt(eps())
         for i = 1:length(p)
             p[i] = real(p[i])
         end

src/types/transferfunction.jl

@@ -115,7 +115,7 @@ tzero(sys::SisoTf) = error("tzero is not implemented for type $(typeof(sys))")
 
 ==(a::SisoTf, b::SisoTf) = ==(promote(a,b)...)
 !=(a::SisoTf, b::SisoTf) = !(a==b)
-isapprox(a::SisoTf, b::SisoTf) = ≈(promote(a,b)...)
+isapprox(a::SisoTf, b::SisoTf; kwargs...) = isapprox(promote(a,b)...; kwargs...)
 #####################################################################
 ##                      Constructor Functions                      ##
 #####################################################################
@@ -360,12 +360,12 @@ function ==(t1::TransferFunction, t2::TransferFunction)
 end
 
 ## Approximate ##
-function isapprox(t1::TransferFunction, t2::TransferFunction)
+function isapprox(t1::TransferFunction, t2::TransferFunction; kwargs...)
     t1, t2 = promote(t1, t2)
     fieldsApprox = [:Ts, :ny, :nu, :matrix]
     fieldsEqual = [:inputnames, :outputnames]
     for field in fieldsApprox
-        if !(getfield(t1, field) ≈ getfield(t2, field))
+        if !(isapprox(getfield(t1, field), getfield(t2, field); kwargs...))
             return false
         end
     end
@@ -377,8 +377,8 @@ function isapprox(t1::TransferFunction, t2::TransferFunction)
     return true
 end
 
-function isapprox{T<:SisoTf, S<:SisoTf}(t1::Array{T}, t2::Array{S})
-    reduce(&, [isapprox(t1[i], t2[i]) for i in eachindex(t1)])
+function isapprox{T<:SisoTf, S<:SisoTf}(t1::Array{T}, t2::Array{S}; kwargs...)
+    reduce(&, [isapprox(t1[i], t2[i]; kwargs...) for i in eachindex(t1)])
 end
 
 ## ADDITION ##

test/test_synthesis.jl

@@ -4,14 +4,23 @@ using ControlSystems
 
 P = tf(1,[1,1])
 C = tf([1,1],[1,0])
+L = P*C
+Lsys = ss(L)
+
 
 B = [1]
 A = [1,1]
 R = [1,1]
 S = [1]
 T = [1]
 
-@test feedback(P,C) == feedback(P*C) == tf([1,2,1,0],[1,3,3,1,0])
+@test isapprox(minreal(feedback(P,C),1e-5), minreal(feedback(L),1e-5))
+@test isapprox(numpoly(minreal(feedback(L),1e-5))[1].a, numpoly(tf(1,[1,1]))[1].a)# This test is ugly, but numerical stability is poor for minreal
 @test feedback2dof(B,A,R,S,T) == tf(B.*T, conv(A,R) + [0;0;conv(B,S)])
 @test feedback2dof(P,R,S,T) == tf(B.*T, conv(A,R) + [0;0;conv(B,S)])
+@test isapprox(pole(minreal(tf(feedback(Lsys)),1e-5)) , pole(minreal(feedback(L),1e-5)), atol=1e-5) 
+
+@test_err feedback(ss(1),ss(1))
+@test_err feedback(ss(eye(2), ones(2,2), ones(1,2),0))
+
 end

test/test_zpk.jl

@@ -32,7 +32,7 @@ z = zpk("z", 0.005)
 @test C_022 == zpk(vecarray(Int64, 2, 2, [], [], [], []), vecarray(Int64, 2, 2, [], [], [], []), [4 0; 0 4])
 @test D_022 == zpk(vecarray(2, 2, [], [], [], []), vecarray(2, 2, [], [], [], []), [4 0; 0 4], 0.005)
 @test C_022 == [zpk(4) 0;0 4]
-#We might want to fix this
+#TODO We might want to fix this
 #@test D_022 == [zpk(4, 0.005) 0;0 4])
 
 #TODO improve polynomial accuracy se these are equal