-
Notifications
You must be signed in to change notification settings - Fork 13
/
SimpleKalman.m
52 lines (40 loc) · 1.85 KB
/
SimpleKalman.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
close all;
global realpos;
global t;
endtime = 100;
time = linspace(1,endtime, 1000);
realpos = sin(time/5);
plot(time, realpos);
hold on;
% The estimated noises of our process (AKA odometry, control), and measurement
processNoise = 0.1;
measurementNoise = 0.1;
x = realpos(1);
p = processNoise;
unfilteredOdometry = x;
for t = 1:length(time)
%-------------------- "Prediction Step" --------------------
u = takeOdometry();
x = x + u;
p = p + processNoise;
%-------------------- "Update Step" --------------------
if mod(t, 10) == 0
z = takeMeasurement();
plot(time(t), z, 'r*');
y = z - x; % Compute the 'residual': i.e. the difference between our estimated position,
% and our absolute position measurement
k = p / (p + measurementNoise); % Compute the 'Kalman gain': i.e. the amount that we should trust this new absolute
% measurement. Notice that when p is large relative to measurementNoise, k will be
% close to 1. When measurement noise is large relative to p, k will be close to 0.
% If p = measurementNoise, then k = 0.5.
x = x + k*y; % Mix in the new absolute measurement proportional to k. If k is 1, then the new x = z.
% If k is 0, then the new x = x. If k is 0.5, then the new x = 0.5*x + 0.5*z.
p = (1 - k)*p; % The new process noise is decreased proportional to k.
end
plot(time(t), x, 'k.');
% Plot the 'unfiltered odometry' in green: i.e. what we would see if we
% just tried to integrate the raw odometry readings without the kalman filter.
unfilteredOdometry = unfilteredOdometry + u;
plot(time(t), unfilteredOdometry, 'go');
pause(0.05);
end