Learning the Kalman Filter in Simulink Examples
Examples to use the Simulink model kalmanfilter. By Yi Cao at Cranfield University on 25 January 2008
The Simulink model shows an example how the Kalman Filter can be implemented in Simulink. The model itself is configured with a Gaussian process connected with a Kalman Filter. To directly use this model, one only needs to provide model prarameters including parameters of the Gaussian process, which are state space matrices, A, B, C, and D, initial state, x0, and covariance matrices, Q and R; and similar parameters for the Kalman Filter, which can be in different values to mimic the model mismatch, plus the state covariance, P. The following examples show how this model can be used.
The Kalman Filter can also be used as a standard model block to be connected with any other systems.
Modified from Michael C. Kleder's "Learning the Kalman Filter"
Define the system as a constant of 12 volts:
But the Kalman filter uses different model.
This example shows the Kalman filter has certain robustness to model uncertainty.
A = 0; B = 1; % this is original definition x(k+1) = 12 + w A1 = 1; B1 = 0; % this is original definition for Kalman filter % Define a process noise (stdev) of 2 volts as the car operates: Q = 2^2; % variance, hence stdev^2 Q1 = Q; % Define the voltmeter to measure the voltage itself: % y(k+1) = x(k+1) + v, v ~ N(0,2^2) C = 1; D = 0; C1 = C; D1 = D; % Define a measurement error (stdev) of 2 volts: R = 2^2; R1 = R; % Define the system input (control) functions: u = 12; % for the constant % Initial state, 12 volts x0 = 12; x1 = x0; %for Kalman filter % Initial covariance as C*R*C' P1 = 2^2; % Simulation time span tspan = [0 100]; [t,x,y] = sim('kalmanfilter',tspan,,[0 u]); figure hold on grid on % plot measurement data: hz=plot(t,y(:,2),'r.','Linewidth',2); % plot a-posteriori state estimates: hk=plot(t,y(:,3),'b-','Linewidth',2); % plot true state ht=plot(t,y(:,1),'g-','Linewidth',2); legend([hz hk ht],'observations','Kalman output','true voltage',0) title('Automobile Voltmeter Example') hold off
Modified from "An Intuitive Introduction to Kalman Filter" by Alex Blekhman.
The train is initially located at the point x = 0 and moves along the X axis with velocity varying around a constant speed 10m/sec. The motion of the train can be described by a set of differential equations:
where x_1 is the position and x_2 is the velocity, w the process noise due to road conditions, wind etc.
Problem: using the position measurement to estimate actual velocity.
Approach: We measure (sample) the position of the train every dt = 0.1 seconds. But, because of imperfect apparature, weather etc., our measurements are noisy, so the instantaneous velocity, derived from 2 consecutive position measurements (remember, we measure only position) is innacurate. We will use Kalman filter as we need an accurate and smooth estimate for the velocity in order to predict train's position in the future.
Model: The state space equation after discretization with sampling time dt
The measurement model is
dt = 0.1; A = expm([0 1;0 0]*dt); B = [0;0]; Q = diag([0;1]); C = [1 0]; D = 0; R = 1; u = 0; x0 = [0;10]; % For Kalman filter, we use the same model A1 = A; B1 = B; C1 = C; D1 = D; Q1 = Q; R1 = R; x1 = [0;5]; %but with different initial state estimate P1 = eye(2); % Run the simulation for 100 smaples tspan = [0 200]; [t,x,y1,y2,y3,y4] = sim('kalmanfilter',tspan,,[0 u]); Xtrue = y1(:,1); %actual position Vtrue = y1(:,2); %actiual velocity z = y2; %measured position X = y3; %Kalman filter output t = t*dt; % actual time % Position analysis figure; subplot(211) plot(t,Xtrue,'g',t,z,'c',t,X(:,1),'m','linewidth',2); title('Position estimation results'); xlabel('Time (s)'); ylabel('Position (m)'); legend('True position','Measurements','Kalman estimated displacement','Location','NorthWest'); % Velocity analysis % The instantaneous velocity as derived from 2 consecutive position % measurements InstantV = [10;diff(z)/dt]; % The instantaneous velocity as derived from running average with a window % of 5 samples from instantaneous velocity WindowSize = 5; InstantVAverage = filter(ones(1,WindowSize)/WindowSize,1,InstantV); % figure; subplot(212) plot(t,InstantV,'g',t,InstantVAverage,'c',t,Vtrue,'m',t,X(:,2),'k','linewidth',2); title('Velocity estimation results'); xlabel('Time (s)'); ylabel('Velocity (m/s)'); legend('Estimated velocity by raw consecutive samples','Estimated velocity by running average','True velocity','Estimated velocity by Kalman filter','Location','NorthWest'); set(gcf,'Position', [100 100 600 800]);
dt = 0.1; A = [0.8110 -0.0348 0.0499 0.3313 0.0038 0.9412 0.0184 0.0399 0.1094 0.0094 0.6319 0.1080 -0.3186 -0.0254 -0.1446 0.8391]; B = [-0.0130 0.0024 -0.0011 0.0100 -0.0781 0.0009 0.0092 0.0138]; C = [0.1685 -0.9595 -0.0755 -0.3771 0.6664 0.0835 0.6260 0.6609]; D = [0.2 0;0 0.1]; % process noise variance Q=diag([0.5^2 0.2^2 0.3^2 0.5^2]); % measurement noise variance R=eye(2); % random initial state x0 = randn(4,1); % Kalman filter set up % The same model A1 =A; B1 = B; C1 = C; D1 = D; Q1 = Q; R1 = R; % However, zeros initial state x1 = zeros(4,1); % Initial state covariance P1 = 10*eye(4); % Simulation set up % time span 100 samples tspan = [0 1000]; % random input change every 100 samples u = [(0:100:1000)' randn(11,2)]; % simulation [t,x,y1,y2,y3,y4] = sim('kalmanfilter',tspan,,u); % plot results % Display results t = t*dt; figure set(gcf,'Position',[100 100 600 800]) for k=1:4 subplot(4,1,k) plot(t,y1(:,k),'b',t,y3(:,k),'r','linewidth',2); legend('Actual state','Estimated state','Location','best') title(sprintf('state %i',k)) end xlabel('time, s')