%  Example 7.11.  Kalman filtering with non white measurement noise.
clear
%  Define the plant and the plant noise.
A=[0 1;-1 -1]; Bw=[0;1]; Cm=[1 0]; Sw=1;
%  Define the filter that generates the measurement noise.
Af=-1/3; Sv1=2/3; R0 = 1;
%  Define the constructed measurement matrix.
C1=Cm*A-Af*Cm;
%  Define the sampling time and a time vector.
dt=0.05;Its=400;
T=0:dt:(2*Its-1)*dt;
%  Generate the sampled plant noise.
w=sqrt(Sw/dt)*randn(1,2*Its);
%  Generate the samples noise that produces the measurement noise.
v1=sqrt(Sv1/dt)*randn(1,2*Its);
%  Generate the measurement noise by filtering.
v=lsim(Af,1,1,0,v1,T);
%  Generate the simulated plant states.
[y,x]=lsim(A,Bw,Cm,0,v,T);
%  Generate the measured output.  Note this output is defined 
%  after the initial transients have decayed to make the 
%  simulation more realistic.
m=y(Its+1:2*Its)+v(Its+1:2*Its);
%  Initialize the Kalman filter an before receiving any measurements.
xh0=[0;0]; P0=lyap(A,Bw*Sw*Bw');
%  Initialize the Kalman filter an instant after receiving the 
%  first measurement.
xh(:,1)=xh0+P0*Cm'*(m(1)-Cm*xh0)/(Cm*P0*Cm'+R0);
P=P0-P0*Cm'*Cm*P0/(Cm*P0*Cm'+R0);
%  Only the diagonal elements of the estimation error covariance 
%  matrix will be saved.
Pd(:,1)=diag(P);
%  The gains are constructed here for convenience.
G(:,1)=P*C1'/Sv1;
Gdot=(P*A'+A*P+Bw*Sw*Bw'-P*C1'*C1*P/Sv1)*C1'/Sv1;
%  xhmgm is x-hat minus G*m(t).
xhmgm(:,1)=xh(:,1)-G(:,1)*m(1);
%  This loop updates the Kalman gain and implements the Kalman 
%  filter
for k=1:Its-1,
   xhmgm(:,k+1)=xhmgm(:,k)+dt*(A*xh(:,k)-Gdot*m(k)-G(:,k)*(Af*m(k)+C1*xh(:,k)));
   P=P+dt*(P*A'+A*P+Bw*Sw*Bw'-P*C1'*C1*P/Sv1);
   Sd(:,k+1)=sqrt(diag(P));
   G(:,k+1)=P*C1'/Sv1;
   Gdot=(P*A'+A*P+Bw*Sw*Bw'-P*C1'*C1*P/Sv1)*C1'/Sv1;
   xh(:,k+1)=xhmgm(:,k+1)+G(:,k+1)*m(k+1);
end
%  Compute the mean square error.
MSE=sum(diag((x(Its+1:2*Its,:)-xh')'*(x(Its+1:2*Its,:)-xh')))
%  Generate a time vector for plotting.
Tp=0:dt:(Its-1)*dt;
%  Plot the results.
set(0,'DefaultAxesFontName','times')
set(0,'DefaultAxesFontSize',16)
set(0,'DefaultTextFontName','times')
figure(1)
clf
subplot(2,2,1)
plot(Tp,x(Its+1:2*Its,1), 'r-',Tp,xh(1,:), 'r--')
legend('Actual', 'Estimate')
xlabel('Time (sec)')
ylabel('Shaft angle')
grid
subplot(2,2,2)
plot(Tp,x(Its+1:2*Its,2), 'r-',Tp,xh(2,:), 'r--')
legend('Actual', 'Estimate')
xlabel('Time (sec)')
ylabel('Angle Rate')
grid
subplot(2,2,3)
plot(Tp, G(1,:), 'r-', Tp, G(2,:), 'r--')
legend('{\itG}_1', '{\itG}_2')
xlabel('Time (sec)')
ylabel('Gains')
grid
subplot(2,2,4)
plot(Tp, Sd(1,:), 'r-', Tp, Sd(2,:), 'r--')
legend('Shaft angle', 'Angle rate')
xlabel('Time (sec)')
ylabel('Standard deviation')
grid
