Skip to content

Instantly share code, notes, and snippets.

@ksvbka
Created July 15, 2015 02:47
Show Gist options
  • Save ksvbka/10c768bb434a49613147 to your computer and use it in GitHub Desktop.
Save ksvbka/10c768bb434a49613147 to your computer and use it in GitHub Desktop.
Example of using Kalman filter for estimates velocity and distance of the vehicle
function kalman(duration, dt)
% function kalman(duration, dt)
%
% Kalman filter simulation for a vehicle travelling along a road.
% INPUTS
% duration = length of simulation (seconds)
% dt = step size (seconds)
measnoise = 10; % position measurement noise (feet)
accelnoise = 0.2; % acceleration noise (feet/sec^2)
a = [1 dt; 0 1]; % transition matrix
b = [dt^2/2; dt]; % input matrix
c = [1 0]; % measurement matrix
x = [0; 0]; % initial state vector
xhat = x; % initial state estimate
Sz = measnoise^2; % measurement error covariance
Sw = accelnoise^2 * [dt^4/4 dt^3/2; dt^3/2 dt^2]; % process noise cov
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 commanded acceleration of 1 foot/sec^2.
u = 1;
% Simulate the linear system.
ProcessNoise = accelnoise * [(dt^2/2)*randn; dt*randn];
x = a * x + b * u + ProcessNoise;
% Simulate the noisy measurement
MeasNoise = measnoise * 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(t,pos, t,posmeas, t,poshat);
grid;
xlabel('Time (sec)');
ylabel('Position (feet)');
title('Figure 1 - Vehicle Position (True, Measured, and Estimated)')
figure;
plot(t,pos-posmeas, t,pos-poshat);
grid;
xlabel('Time (sec)');
ylabel('Position Error (feet)');
title('Figure 2 - Position Measurement Error and Position Estimation Error');
figure;
plot(t,vel, t,velhat);
grid;
xlabel('Time (sec)');
ylabel('Velocity (feet/sec)');
title('Figure 3 - Velocity (True and Estimated)');
figure;
plot(t,vel-velhat);
grid;
xlabel('Time (sec)');
ylabel('Velocity Error (feet/sec)');
title('Figure 4 - Velocity Estimation Error');
@oguz241
Copy link

oguz241 commented Dec 24, 2022

it doesnt press graph can u help me ı save this code than kalman(1,100) ı did it bu it doesn't work

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment