Question

a Matlab code for Kalman Filter to combine GPS and Odometer

Answer 1

x is the GPS 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 Odometer, 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