% Extended Kalman Filter Demo
%{
Version 1.0, September 2006
This tutorial was written by Jose Manuel Rodriguez,
Universidad Politecnica de Cataluņa, Spain
Based on the Technical Document by
Juan Andrade-Cetto, "The Kalman Filter", March 2002
%}
%{
x is the real plant behavior, in this case a sinus wave
with the following formulation:
x(time)=sin(frec(@ time-1)*time-1) + ramdom error signal("sigmav")
frec(time)=frec(@ time-1)
x_ is the predicted state, this is where Kalman filter will come
and where we will correct our estimations using an observation
z is the observation of the real plant, in this case corresponding only to
the position at a given time. Note that this observation is subject to an
error, therefore the resulting equation is: z(time)=x(time)+ramdom error("sigmaw")
Our first prediction will come from the plant ideal behavior, then
using the observations and error covariance we will obtain a better estimate.
xc is the ideal plant behavior... this is used just for comparison
P is the state error covariances at a given time of all the involved variables,
note that we are forming x as a 2 by 1 matrix with the following:
x(1,n) -> position
x(2,n) -> frecuency
Our functions are as following for this example:
Let's say:
f1: x1(time)=sin(x2(time-1)*(time-1))+V V->ramdom plant error
f2: x2(time)=x2(time-1)
h: y=x1+w w->ramdom sensor error
F is the Jacobian of the transfer function due to the involved variables,
in this case these are x1 and x2, therefore F will be a 2 by 2 matrix
(always the matrix is square). The resulting F depends on time and must be
computed for every step that the system takes.
F is as follows:
F -> df1/dx1 = 0 df1/dx2 = cos(x2*time)*time
df2/dx1 = 0 df2/dx1 = 1
%}
clear all; close all;
% Initial Conditions
x(:,1) = [0;0.05]; %Our real plant initial condition
x_(:,1) = [0;0.04]; %Our estimate initial conidition (they might differ)
xc = x_; %Set the ideal model as we think it should start
P = [0.01 0; %set initial error covariance for position & frec, both at sigma 0.1, P=diag([sigma_pos_init^2 sigmav_frec_init^2])
0 0.01];
sigmav = 0.1; %the covariance coeficient for the position error, sigma
sigmaw = 0.5; %the covariance coeficient for the frecuency error, sigma
Q = sigmav*sigmav; %the error covariance constant to be used, in this case just a escalar unit
R = sigmaw*sigmaw; %the error covariance constant to be used, in this case just a escalar unit
G = [1;0]; %G is the Jacobian of the plant tranfer functions due to the error.
H = [ 1 0]; %H is the Jacobian of the sensor transfer functions due to the variables involved
W = 1; %W is the Jacobian of the sensor transfer functions due to the error.
steps = 1000; %Amount of steps to simulate
% bucle
for i =2:steps %start @ time=2
% the real plant
x(:,i) = [sin(x(2,i-1)*(i-1)) + randn*sigmav ; x(2,i-1) ];
z(i) = x(1,i) + randn*sigmaw;
% blind prediction (just for comparison)
xc(:,i) = [sin(xc(2,i-1)*(i-1)); xc(2,i-1)];
% prediction
x_(:,i) = [sin(x_(2,i-1)*(i-1)); x_(2,i-1)];
z_(i) = x_(1,i);
% compute F
F = [0 i*cos(x_(2,i)*i);
0 1];
% Prediction of the plant covariance
P = F*P*F' + G*Q*G';
% Innovation Covariance
S = H*P*H'+R;
% Kalman's gain
K = P*H'*inv(S);
% State check up and update
x_(:,i) = x_(:,i) + K * (z(i)-z_(i));
% Covariance check up and update
P = (eye(2)-K*H)*P;
sigmaP(:,i)=sqrt(diag(P)); %sigmap is for storing the current error covariance for ploting pourposes
end
figure(1);clf; hold on;
plot(x(1,:),'-b'); %plot the real plant behavior
plot(z,'.r'); %plot the observations over this plant
plot(x_(1,:),'-g'); %plot the Kalman filter prediction over the plant
plot(xc(1,:),'-m'); %The original thought of the plant
plot(x_(1,:)+2*sigmaP(1,:),'-g'); %These two are the threshold in witch I'm certain that the plant state is at a given time
plot(x_(1,:)-2*sigmaP(1,:),'-g');
figure(2);clf;hold on;
plot(x(2,:),'-b'); %Frecuency estimation
plot(x_(2,:),'-g'); %Frecuency filtered by Kalman