From b75faea01b2808c4eb0875d310c80f339e43290a Mon Sep 17 00:00:00 2001
From: Fredrik Bagge Carlson <baggepinnen@gmail.com>
Date: Fri, 28 May 2021 07:32:40 +0200
Subject: [PATCH] add controller function (#512)

* add controller function

* rename controller and predictor
---
 src/ControlSystems.jl     |  3 ++-
 src/matrix_comps.jl       | 34 ++++++++++++++++++++++++++--------
 test/test_matrix_comps.jl | 31 ++++++++++++++++++++++++++-----
 3 files changed, 54 insertions(+), 14 deletions(-)

diff --git a/src/ControlSystems.jl b/src/ControlSystems.jl
index af146fc5c..949500aa1 100644
--- a/src/ControlSystems.jl
+++ b/src/ControlSystems.jl
@@ -41,7 +41,8 @@ export  LTISystem,
         similarity_transform,
         prescale,
         innovation_form,
-        predictor,
+        observer_predictor,
+        observer_controller,
         # Stability Analysis
         isstable,
         pole,
diff --git a/src/matrix_comps.jl b/src/matrix_comps.jl
index 7b168552d..f92a85637 100644
--- a/src/matrix_comps.jl
+++ b/src/matrix_comps.jl
@@ -639,11 +639,11 @@ function innovation_form(sys::ST; sysw=I, syse=I, R1=I, R2=I) where ST <: Abstra
 end
 
 """
-    predictor(sys::AbstractStateSpace, R1, R2)
-    predictor(sys::AbstractStateSpace, K)
+    observer_predictor(sys::AbstractStateSpace, R1, R2)
+    observer_predictor(sys::AbstractStateSpace, K)
 
-Return the predictor system
-x̂' = (A - KC)x̂ + Bu + Ky
+Return the observer_predictor system
+x̂' = (A - KC)x̂ + (B-KD)u + Ky
 ŷ  = Cx + Du
 with the input equation [B K] * [u; y]
 
@@ -651,12 +651,30 @@ If covariance matrices `R1, R2` are given, the kalman gain `K` is calculaded.
 
 See also `innovation_form`.
 """
-function predictor(sys::ST, R1, R2) where ST <: AbstractStateSpace
+function observer_predictor(sys::ST, R1, R2) where ST <: AbstractStateSpace
     K = kalman(sys, R1, R2)
-    predictor(sys, K)
+    observer_predictor(sys, K)
 end
 
-function ControlSystems.predictor(sys, K::AbstractMatrix)
+function observer_predictor(sys, K::AbstractMatrix)
     A,B,C,D = ssdata(sys)
-    ss(A-K*C, [B K], C, [D zeros(size(D,1), size(K, 2))], sys.timeevol)
+    ss(A-K*C, [B-K*D K], C, [D zeros(size(D,1), size(K, 2))], sys.timeevol)
+end
+
+"""
+    cont = observer_controller(sys, L::AbstractMatrix, K::AbstractMatrix)
+
+Return the observer_controller `cont` that is given by
+`ss(A - B*L - K*C + K*D*L, K, L, 0)`
+
+Such that `feedback(sys, cont)` produces a closed-loop system with eigenvalues given by `A-KC` and `A-BL`.
+
+# Arguments:
+- `sys`: Model of system
+- `L`: State-feedback gain `u = -Lx`
+- `K`: Observer gain
+"""
+function observer_controller(sys, L::AbstractMatrix, K::AbstractMatrix)
+    A,B,C,D = ssdata(sys)
+    ss(A - B*L - K*C + K*D*L, K, L, 0, sys.timeevol)
 end
diff --git a/test/test_matrix_comps.jl b/test/test_matrix_comps.jl
index 4b5144795..3eb5864a8 100644
--- a/test/test_matrix_comps.jl
+++ b/test/test_matrix_comps.jl
@@ -106,10 +106,31 @@ sysi = ControlSystems.innovation_form(sys, sysw=sysw)
  40.26132476965486]
 
 
-# Test innovation form
-sysp = ControlSystems.predictor(sys, I(2), I(1))
+# Test observer_predictor
+sysp = ControlSystems.observer_predictor(sys, I(2), I(1))
 K = kalman(sys, I(2), I(1))
 @test sysp.A == sys.A-K*sys.C
-@test sysp.B == [sys.B K]
-
-end
+@test sysp.B == [sys.B-K*sys.D K]
+
+
+# Test observer_controller
+sys = ssrand(2,3,4)
+Q1 = I(4)
+Q2 = I(3)
+R1 = I(4)
+R2 = I(2)
+L = lqr(sys, Q1, Q2)
+K = kalman(sys, R1, R2)
+cont = observer_controller(sys, L, K)
+syscl = feedback(sys, cont)
+
+pcl = pole(syscl)
+A,B,C,D = ssdata(sys)
+allpoles = [
+    eigvals(A-B*L)
+    eigvals(A-K*C)
+]
+@test sort(pcl, by=LinearAlgebra.eigsortby) ≈ sort(allpoles, by=LinearAlgebra.eigsortby) 
+@test cont.B == K
+
+end