.Modelica_LinearSystems2.StateSpace.Design.kalmanFilter

Information

Syntax

(L, kss) = StateSpace.Design.kalmanFilter(ss, Q, R)

Description

This functions designs the kalman-bucy filter, that reconstructs plant states and system output without noise. As input it uses the plant input and output.

Noise affects the plant states via q(t)

dx/dt = Ax + Bu + q(t)

The plant output is affected by r(t)

y = Cx + Du + r(t)

The covariance matrices of q and r have to be given via Q and R, respectively.

The filter uses an observer that tries to reconstruct the original behaviour. Its states and outputs are trailed with a hat (^)
The observer is controlled by feedback of the output difference y - y^ (y^= Cx^+ Du) over a Matrix L, such that x^

dx^/dt = (A - LC) x^ + (B - LD)u + Ly

follows the plant state x. L is designed to minimize noise in states and inputs. L is calculated from a Riccati Equation

    -1
SC'R  CS - SA' - AS - Q = 0
        -1
L = SC'R

The representation of the estimation model would be as follows:

.                            |u|
x^  = [A-LC] x^ + [B-LD , L] | |
                             |y|
                    |u|
y^ = [C] x^ + [D 0] | |
                    |y|

Since the controller approach was made to provide the estimated states, the representation of the output kss is such that

y^ = x^

i.e., kss:

.                            |u|
x^  = [A-LC] x^ + [B-LD , L] | |
                             |y|
            |u|
y^ = Ix^ + 0| |
            |y|

i.e.

C^ = I,   D^ = 0   with appropriate sizes, i.e. size(C^) = {nx,nx},  size(D^) = {nx, nu+ny}.

Since the calculation of a Kalman filter is the dual problem of lqr calculation function Modelica_LinearSystems2.StateSpace.Design.lqr is used to solve the Riccati euation.
The algebraic Riccati equation is solved by using the Schur algorithm care.

Example

  import Modelica_LinearSystems2.StateSpace;
  import Modelica_LinearSystems2.TransferFunction;
  StateSpace css = StateSpace(TransferFunction.constructor({1},{1,2,3,4}));
  Real Q[:,:] = identity(3);
  Real R[:,:] = [1];

  Real L[:,:];
  StateSpace kss(
            redeclare Real A[size(css.A,1),size(css.A,1)],
            redeclare Real B[size(css.B,1),size(css.B,2)+size(css.C,1)],
            redeclare Real C[size(css.C,1)+size(css.A,1),size(css.C,2)],
            redeclare Real D[size(css.C,1)+size(css.A,1),size(css.B,2)+size(css.C,1)]);

algorithm
  ( L, kss) := StateSpace.Design.kalmanFilter(css, Q, R);
//  L = [0.9928;
       (-0.0072);
       (-1.4868)]
// kss = StateSpace(
          A = [-0.993,     1,     0;
                0.007,     0,     1;
               -2.513,     3,    -2],

          B = [ 0,         0.993;
                0,        -0.0072;
                1,        -1.4868],

          C = [ 1,     0,     0;
                0,     1,     0;
                0,     0,     1],

          D = [ 0,     0;
                0,     0;
                0,     0])

Interface

function kalmanFilter
  import Modelica_LinearSystems2.StateSpace;
  input StateSpace ss "Time-continuous system in state space form";
  input Real Q[size(ss.A, 1), size(ss.A, 1)] "Covariance Matrix of state noise (n x n), n number of states";
  input Real R[size(ss.C, 1), size(ss.C, 1)] "Covariance Matrix of output noise (m x m), m number of inputs";
  output Real L[:, :] "Kalman filter matrix";
  output StateSpace kss(redeclare Real A[size(ss.A, 1), size(ss.A, 1)], redeclare Real B[size(ss.B, 1), size(ss.B, 2) + size(ss.C, 1)], redeclare Real C[size(ss.A, 1), size(ss.A, 2)], redeclare Real D[size(ss.A, 1), size(ss.B, 2) + size(ss.C, 1)]) "Kalman system";
end kalmanFilter;

Revisions