function kalman(duration,dt)
%function kalman(duration,dt)
%
%Kalman filter simulation for a vehicle travelling along a road.
%INPUTS
% duration=lenght of simulation (seconds)
% dt=step size (seconds)
meannoise=10; %position measurement noise (feet)
accelnoise=0.2; %acceleration noise (feet/sec^2)
dt=0.1;duration=60;
a=[0 0 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0 0;0 0 0 0.85 -0.425 -0.425 0.18 1.32 0 0 0 0;0 0 0 0 0.5 4.76 0 5 0 0 0 0;0 0 0 0.5 0 3 -4.25 4.25 0 0 0 0;0 0 0 4.76 -3 0 2.5 4.25 0 0 0 0;0 0 0 0 0 0 3.8 5.61 0 1 0.5 0.85;0 0 0 0 0 0 -3.23 0 0 0 0.85 -0.5;0 0 0 0 0 0 1.93 -7.7 0 0 0.56 1;0 0 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0 0]; %transition matrix
b=[0 0 0;0 0 0;0 0 0;dt 0 0;0 dt 0;0 0 dt;0 0 0;0 0 0;0 0 0;0 0 0;0 0 0;0 0 0]; %input matrix
c=[1 0 0 0 0 0 0 0 0 0 0 0;0 1 0 0 0 0 0 0 0 0 0 0 ;0 0 1 0 0 0 0 0 0 0 0 0]; %measurement matrix
x=[0;0;0;0;0;0;0;0;0;0;0;0]; %initial state vector
xhat=x; %initial state estimate
Sz=meannoise^2; %measurement error covariance
Sw=accelnoise^2*[0 0 0 dt dt dt 0 0 0 0 0 0;0 0 0 dt dt dt 0 0 0 0 0 0;0 0 0 dt dt dt 0 0 0 0 0 0;dt dt dt dt^2 dt^2 dt^2 dt dt dt dt dt dt;dt dt dt dt^2 dt^2 dt^2 dt dt dt dt dt dt;dt dt dt dt^2 dt^2 dt^2 dt dt dt dt dt dt;0 0 0 dt dt dt 0 0 0 0 0 0;0 0 0 dt dt dt 0 0 0 0 0 0;0 0 0 dt dt dt 0 0 0 0 0 0;0 0 0 dt dt dt 0 0 0 0 0 0;00 0 0 dt dt dt 0 0 0 0 0 0;0 0 0 dt dt dt 0 0 0 0 0 0]; %process noise covariance
P=Sw; %initial estimation covariance
%initialize arrays for later plotting.
pos=[]; %true position array
poshat=[]; %estimated position array
posmeas=[]; %measured position array
vel=[]; %true velocity array
velhat=[]; %estimated velocity array
for t=0:dt:duration,
%use a constant command acceleration of 1 foot/sec^2.
u=[1;1;1];
%simulate the linear system
ProcessNoise=accelnoise*[1*randn;1*randn;1*randn;dt*randn;dt*randn;dt*randn;1*randn;1*randn;1*randn;1*randn;1*randn;1*randn];
x=a*x;
%simulate the noisy measurement
MeasNoise=meannoise*randn;
y=c*x+MeasNoise;
%Extrapolate the most recent state estimate to the present time.
xhat=a*xhat+b*u;
%Form the Innovation vector.
Inn=y-c*xhat;
%Compute the covariance of the Innovation.
s=c*P*c'+Sz;
%Form the Kalman Gain Matrix.
K=a*P*c'*inv(s);
%Update the state estimate.
xhat=xhat+K*Inn;
%Compute the covariance of the estimation error.
P=a*P*a'-a*P*c'*inv(s)*c*P*a'+Sw;
%Save some parameters for plotting later.
pos=[pos;x(1)];
posmeas=[posmeas;y];
poshat=[poshat;xhat(1)];
vel=[vel;x(2)];
velhat=[velhat;xhat(2)];
end
%Plot the results
close all;
t=0:dt:duration;
figure;
plot(tout,pos)