lqe linear quadratic estimator (Kalman Filter)

## Syntax

              [K, X] = lqe(Pw)
              [K, X] = lqe(P, Qww, Rvv)
              [K, X] = lqe(P, Qww, Rvv, Swv)

## Arguments

Pw

  A state space representation of a linear dynamical system (see syslin)

P

  A state space representation of a linear dynamical system (`nu` inputs, `ny` outputs, `nx` states) (see syslin)

Qww

  Real `nx` by `nx` symmetric matrix, the process noise variance

Rvv

  full rank real `ny` by `ny` symmetric matrix, the measurement noise variance.

Swv

  real `nx` by `ny` matrix, the process noise vs measurement noise covariance. The default value is zeros(nx,ny).

K

  a real matrix, the optimal gain.

X

  a real symmetric matrix, the stabilizing solution of the Riccati equation.

## Description

This function computes the linear optimal LQ estimator gain of the state estimator for a detectable (see dt_ility) linear dynamical system and the variance matrices for the process and the measurement noises.

### Syntax `[K,X]=lqe(P,Qww,Rvv [,Swv])`

Computes the linear optimal LQ estimator gain K for the dynamical system: \text{P}=\left\lbrace \begin{array}{l}\dot{x}=A x+B u+w\\y=C x+D u +v\end{array}\right. \text{ in continuous time}

or

\text{P}=\left\lbrace \begin{array}{l}x^+=A x+B u +w\\y=C x+D u +v\end{array}\right. \text{ in discrete time}

Where w and v are white noises such as \mathbb{E}(w w')=Q_{ww} \text{ , }\mathbb{E}(v v')=R_{vv} \text{ and }\mathbb{E}w v')=S_{wv}

The values of B and D are not taken into account.

#### Standard form

\mathbb{E}\left(\left[\begin{array}{l}w\\v\end{array}\right]\left[\begin{array}{ll}w'&v'\end{array}\right]\right)=\left[\begin{array}{ll}Q_{ww}&S_{wv}\\S_{wv}'&R_{vv}\end{array}\right] This covariance matrix can be factored using full-rank factorization (see fuffrf) as \left[\begin{array}{l}B_w\\D_w\end{array}\right] \left[\begin{array}{ll}B_w'&D_w'\end{array}\right] And consequently the initial dynamical system is equivalent to \left\{\begin{array}{l}\dot{x}=A x+B u +B_w w \\y=C x+D u+D_w w\end{array}\right. \text{ in continuous time } or \left\{\begin{array}{l}x^+=A x+B u+B_w w\\y=C x+D u+D_w w \end{array}\right. \text{ in discrete time.} Where w is now a white noise such as \mathbb{E}(w w')=I

### Syntax `[K,X]=lqe(Pw)`

Computes the linear optimal LQ estimator gain K for the dynamical system \text{Pw}=\left\{\begin{array}{l}\dot{x}=A x+B_w w\\z=C x+D_w w\end{array}\right. \text{ in continuous time or }\text{Pw}=\left\{\begin{array}{l}x^+=A x+B_w w\\z=C x+D_w w\end{array}\right. \text{ in discrete time.}

Where w is a white noise with unit covariance.

### Properties

- A + K.C is stable.

- the state estimator is given by the dynamical system: \dot{\hat{x}}=(A+K C)\hat{x}+(B_w+K D_w) u -K y \text{ in continuous time }

  or

  \hat{x}^+=(A+K C)\hat{x}+(B_w+K D_w) u -K y \text{ in discrete time.} It minimizes the covariance of the steady state error x-\hat{x}.

- For discrete time systems the state estimator is such that: \hat{x}_{k+1}= \mathbb{E}(x_k| y_0,...,y_k) (one-step predicted x).

### Algorithm

Let Q = Bw Bw', R = Dw Dw' and S = Bw Dw'

- For the continuous time case K is given by: K = -(X.C'+ S).R-1

  where `X` is the solution of the stabilizing Riccati equation

  (A - SR-1C)X + X(A - SR-1C)' - XC'R-1CX + Q - SR-1S'= 0

- For the discrete time case K is given by K = - (A X C'+S)(C X C'+ R)+

  where `X` is the solution of the stabilizing Riccati equation

  AXA'- X - (AXC' + S)(CXC' + R)+(CXA' + S') + Q

## Examples

Assume the dynamical system formed by two masses connected by a spring and a damper:

A force u=\bar{u}+e (where e is a noise) is applied to the big one, the deviations from equilibrium positions dy1 and dy2 of the masses are measured. These measures are also subject to an additional noise v.

A continuous time state space representation of this system is: \left\lbrace\begin{array}{l} \dot{x}=\left[\begin{array}{llll}0&1&0&0\\ -k/M&-b/M&k/M&b/M\\ 0&0&0&1\\ k/m&b/m&-k/m&-b/m \end{array}\right] x +\left[\begin{array}{l}0\\ 1/M\\ 0\\ 0 \end{array}\right] (\bar{u}+e)\\ \left[\begin{array}{l}dy_1\\ dy_2\end{array}\right]=\left[\begin{array}{llll}1&0&0&0\\ 0&0&1&0 \end{array}\right] x +v \end{array}\right. \text{Where }x=\left[\begin{array}{l}dy_1\\ \dot{dy_1}\\ dy_2\\ \dot{dy_2}\end{array}\right] and e and v are discrete time white noises such as \mathbb{E}(e e')=Q_e \text{ , }\mathbb{E}(v v')=R_vv \text{ and }\mathbb{E}(e v')=S_{ev}

The instructions below can be used to compute a LQ state observer of the discretized version of this dynamical system.

    // Form the state space model
    M = 1; m = 0.2; k = 0.1; b = 0.004;
    A = [  0     1    0    0
          -k/M  -b/M  k/M  b/M
           0     0    0    1
          k/m  b/m  -k/m  -b/m];
    B = [0; 1/M; 0; 0];
    C = [1 0 0 0  //dy1
         0 0 1 0];//dy2
    //inputs u and e; outputs dy1 and dy2
    P = syslin("c",A, B, C);
    // Discretize it
    dt=0.5;
    Pd=dscr(P, dt);

    //  Set the noise covariance matrices
    Q_e=0.01; //additive input noise variance
    R_vv=0.001*eye(2,2); //measurement noise variance
    Q_ww=Pd.B*Q_e*Pd.B'; //input noise adds to regular input u

    //----syntax [K,X]=lqe(P,Qww,Rvv [,Swv])---
    Ko=lqe(Pd,Q_ww,R_vv); //observer gain

    //----syntax [K,X]=lqe(P21)---
    bigR =blockdiag(Q_ww, R_vv);
    [W,Wt]=fullrf(bigR);
    Bw=W(1:size(Pd.B,1),:);
    Dw=W(($+1-size(Pd.C,1)):$,:);
    Pw=syslin(Pd.dt,Pd.A,Bw,Pd.C,Dw);
    Ko1=lqe(Pw); //same observer gain

    //Check gains equality
    norm(Ko-Ko1,1)

    // Form the observer
    O=observer(Pd,Ko);
    //check stability
    and(abs(spec(O.A))<1)
    // Check by simulation
    // Modify Pd to make it return the state
    Pdx=Pd;Pdx.C=eye(4,4);Pdx.D=zeros(4,1);
    t=0:dt:30;
    u=zeros(t);
    x=flts(u,Pdx,[1;0;0;0]);//state evolution
    y=Pd.C*x;
    // simulate the observer
    x_hat=flts([u+0.01*rand(u);y+0.0001*rand(y)],O);
    clf;
    subplot(2,2,1)
      plot2d(t',[x(1,:);x_hat(1,:)]'),
      e=gce();e.children.polyline_style=2;
      L=legend('$x_1=dy_1$', '$\hat{x_1}$');L.font_size=3;
      xlabel('Time (s)')
    subplot(2,2,2)
      plot2d(t',[x(2,:);x_hat(2,:)]')
      e=gce();e.children.polyline_style=2;
      L=legend('$x_2=dy_1^+$', '$\hat{x_2}$');L.font_size=3;
      xlabel('Time (s)')
    subplot(2,2,3)
      plot2d(t',[x(3,:);x_hat(3,:)]')
      e=gce();e.children.polyline_style=2;
      L=legend('$x_3=dy_2$', '$\hat{x_3}$');L.font_size=3;
      xlabel('Time (s)')
    subplot(2,2,4)
      plot2d(t',[x(4,:);x_hat(4,:)]')
      e=gce();e.children.polyline_style=2;
      L=legend('$x_4=dy_2^+$', '$\hat{x_4}$');L.font_size=3;
      xlabel('Time (s)')

// Form the state space model M = 1; m = 0.2; k = 0.1; b = 0.004; A = [ 0 1 0 0 -k/m -b/m k/m b/m 0 0 0 1 k/M b/M -k/M -b/M]; B = [0; 0; 0; 1/M]; C = [1 0 0 0 //dy1 0 0 1 0];//dy2 //inputs u and e; outputs dy1 and dy2 P = syslin("c",A, B, C); // Discretize it dt=0.5; Pd=dscr(P, dt); // Set the noise covariance matrices Q_e=0.01; //additive input noise variance R_vv=0.001*eye(2,2); //measurement noise variance Q_ww=Pd.B*Q_e*Pd.B'; //input noise adds to regular input u //----syntax [K,X]=lqe(P,Qww,Rvv [,Swv])--- Ko=lqe(Pd,Q_ww,R_vv); //observer gain //----syntax [K,X]=lqe(P21)--- bigR =blockdiag(Q_ww, R_vv); [W,Wt]=fullrf(bigR); Bw=W(1:size(Pd.B,1),:); Dw=W(($+1-size(Pd.C,1)):$,:); Pw=syslin(Pd.dt,Pd.A,Bw,Pd.C,Dw); Ko1=lqe(Pw); //same observer gain //Check gains equality norm(Ko-Ko1,1) // Form the observer O=observer(Pd,Ko); //check stability and(abs(spec(O.A))<1) // Check by simulation // Modify Pd to make it return the state Pdx=Pd;Pdx.C=eye(4,4);Pdx.D=zeros(4,1); t=0:dt:30; u=zeros(t); x=flts(u,Pdx,[1;0;0;0]);//state evolution y=Pd.C*x; // simulate the observer x_hat=flts([u+0.01*rand(u);y+0.0001*rand(y)],O); clf; subplot(2,2,1) plot2d(t',[x(1,:);x_hat(1,:)]'), e=gce();e.children.polyline_style=2; L=legend('$x_1=dy_1$', '$\hat{x_1}$');L.font_size=3; xlabel('Time (s)') subplot(2,2,2) plot2d(t',[x(2,:);x_hat(2,:)]') e=gce();e.children.polyline_style=2; L=legend('$x_2=dy_1^+$', '$\hat{x_2}$');L.font_size=3; xlabel('Time (s)') subplot(2,2,3) plot2d(t',[x(3,:);x_hat(3,:)]') e=gce();e.children.polyline_style=2; L=legend('$x_3=dy_2$', '$\hat{x_3}$');L.font_size=3; xlabel('Time (s)') subplot(2,2,4) plot2d(t',[x(4,:);x_hat(4,:)]') e=gce();e.children.polyline_style=2; L=legend('$x_4=dy_2^+$', '$\hat{x_4}$');L.font_size=3; xlabel('Time (s)')

## Reference

Engineering and Scientific Computing with Scilab, Claude Gomez and al.,Springer Science+Business Media, LLC,1999, ISNB:978-1-4612-7204-5

## See also

- lqr
- observer
- obscont
- lqg
- fullrf

## History

6.0 lqe(P,Qww,Rvv) and lqe(P,Qww,Rvv,Swv) syntaxes added.