📄 feedbackviewkf.html
字号:
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"><html xmlns:mwsh="http://www.mathworks.com/namespace/mcode/v1/syntaxhighlight.dtd"> <head> <meta http-equiv="Content-Type" content="text/html; charset=utf-8"> <!--This HTML is auto-generated from an M-file.To make changes, update the M-file and republish this document. --> <title>Learning the Kalman Filter: a Feedback Perspective</title> <meta name="generator" content="MATLAB 7.5"> <meta name="date" content="2008-02-27"> <meta name="m-file" content="feedbackviewKF"><style>body { background-color: white; margin:10px;}h1 { color: #990000; font-size: x-large;}h2 { color: #990000; font-size: medium;}/* Make the text shrink to fit narrow windows, but not stretch too far in wide windows. */ p,h1,h2,div.content div { max-width: 600px; /* Hack for IE6 */ width: auto !important; width: 600px;}pre.codeinput { background: #EEEEEE; padding: 10px;}@media print { pre.codeinput {word-wrap:break-word; width:100%;}} span.keyword {color: #0000FF}span.comment {color: #228B22}span.string {color: #A020F0}span.untermstring {color: #B20000}span.syscmd {color: #B28C00}pre.codeoutput { color: #666666; padding: 10px;}pre.error { color: red;}p.footer { text-align: right; font-size: xx-small; font-weight: lighter; font-style: italic; color: gray;} </style></head> <body> <div class="content"> <h1>Learning the Kalman Filter: a Feedback Perspective</h1> <introduction> <p>A simulink model of Kalman filter organized as a feedback control system</p> </introduction> <h2>Contents</h2> <div> <ul> <li><a href="#1">The Kalman Filter</a></li> <li><a href="#2">A Feedback View of the Kalman Filter</a></li> <li><a href="#3">The Simulink model</a></li> <li><a href="#4">Example 1, Automobile Voltimeter</a></li> <li><a href="#5">Example 2, Predict the position and velocity of a moving train,</a></li> <li><a href="#6">Example 3: A 2-input 2-output 4-state system</a></li> <li><a href="#7">Constant Kalman Filter Gain</a></li> <li><a href="#8">Time-Invariant Kalman Filter</a></li> </ul> </div> <h2>The Kalman Filter<a name="1"></a></h2> <p>For a linear Gassian process with input u, output z and internal state, x</p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq157956.png"> </p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq59295.png"> </p> <p>The Kalman filter is normally represented in two stages: a predict stage,</p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq152588.png"> </p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq105662.png"> </p> <p>and a update stage</p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq120013.png"> </p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq76971.png"> </p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq53986.png"> </p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq136047.png"> </p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq77104.png"> </p> <h2>A Feedback View of the Kalman Filter<a name="2"></a></h2> <p>However, to view the Kalman filter as a feedback control system may provide more insight to understand its principle. Re-arrange the seven equations of the Kalman filter described above into the following groups: </p> <p>Process model group:</p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq152588.png"> </p> <p>Kalman filter gain group:</p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq105662.png"> </p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq76971.png"> </p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq53986.png"> </p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq77104.png"> </p> <p>Estimation error:</p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq120013.png"> </p> <p>Feedback control:</p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq136047.png"> </p> <p>The re-organized equations show that the Kalman filter is a feedback control system, where the plant is the process model itsself, the controller is a time varying gain, whilst the measurement of the actual process is a reference signal to the feedback system. From this point of view, if the closed-loop system is stable, then eventually the estimation error will be zero. </p> <p>It is worth to note that the Kalman filter gain only depends on model prarameters and the initial covariance (P0). It is independent from realtime measurment. Hence, the Kalman filter is a time-variant linear system. </p> <h2>The Simulink model<a name="3"></a></h2> <p>The above formulation is implemented in Simulink with different groups are encapsulated into subsystems so that the feedback relationship is clearly shown. </p> <p><img vspace="5" hspace="5" src="feedbackKF.png"> </p> <p>The following results show that this model produces exactly the same results as <a href="http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=18465&objectType=FILE">the Kalman filter simulink model</a> does. </p> <h2>Example 1, Automobile Voltimeter<a name="4"></a></h2> <p>Modified from <a href="http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=5377&objectType=file">Michael C. Kleder's "Learning the Kalman Filter"</a></p> <p>Define the system as a constant of 12 volts:</p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq39812.png"> </p> <p>But the Kalman filter uses different model.</p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq42551.png"> </p> <p>This example shows the Kalman filter has certain robustness to model uncertainty.</p><pre class="codeinput">A = 0; B = 1; <span class="comment">% this is original definition x(k+1) = 12 + w</span>A1 = 1; B1 = 0; <span class="comment">% this is original definition for Kalman filter</span><span class="comment">% Define a process noise (stdev) of 2 volts as the car operates:</span>Q = 2^2; <span class="comment">% variance, hence stdev^2</span>Q1 = Q;<span class="comment">% Define the voltimeter to measure the voltage itself:</span><span class="comment">% y(k+1) = x(k+1) + v, v ~ N(0,2^2)</span>C = 1; D = 0;C1 = C; D1 = D;<span class="comment">% Define a measurement error (stdev) of 2 volts:</span>R = 2^2;R1 = R;<span class="comment">% Define the system input (control) functions:</span>u = 12; <span class="comment">% for the constant</span><span class="comment">% Initial state, 12 volts</span>x0 = 12;x1 = x0; <span class="comment">%for Kalman filter</span><span class="comment">% Initial covariance as C*R*C'</span>P1 = 2^2;<span class="comment">% Simulation time span</span>tspan = [0 100];[t,x,y] = sim(<span class="string">'feedbackkf'</span>,tspan,[],[0 u]);figurehold <span class="string">on</span>grid <span class="string">on</span><span class="comment">% plot measurement data:</span>hz=plot(t,y(:,2),<span class="string">'r.'</span>,<span class="string">'Linewidth'</span>,2);<span class="comment">% plot a-posteriori state estimates:</span>hk=plot(t,y(:,3),<span class="string">'b-'</span>,<span class="string">'Linewidth'</span>,2);<span class="comment">% plot true state</span>ht=plot(t,y(:,1),<span class="string">'g-'</span>,<span class="string">'Linewidth'</span>,2);legend([hz hk ht],<span class="string">'observations'</span>,<span class="string">'Kalman output'</span>,<span class="string">'true voltage'</span>,0)title(<span class="string">'Automobile Voltimeter Example'</span>)hold <span class="string">off</span></pre><img vspace="5" hspace="5" src="feedbackviewKF_01.png"> <h2>Example 2, Predict the position and velocity of a moving train,<a name="5"></a></h2> <p>Modified from <a href="http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=13479&objectType=file">"An Intuitive Introduction to Kalman Filter"</a> by Alex Blekhman. </p> <p>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: </p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq13019.png"> </p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq47464.png"> </p> <p>where x_1 is the position and x_2 is the velocity, w the process noise due to road conditions, wind etc.</p> <p>Problem: using the position measurement to estimate actual velocity.</p> <p>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. </p> <p>Model: The state space equation after discretization with sampling time dt</p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq488660.png"> </p> <p>The measurment model is</p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq62371.png"> </p><pre class="codeinput">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];<span class="comment">% For Kalman filter, we use the same model</span>A1 = A;B1 = B;C1 = C;D1 = D;Q1 = Q;R1 = R;x1 = [0;5]; <span class="comment">%but with different initial state estimate</span>P1 = eye(2);<span class="comment">% Run the simulation for 100 smaples</span>tspan = [0 200];[t,x,y1,y2,y3,y4] = sim(<span class="string">'feedbackkf'</span>,tspan,[],[0 u]);Xtrue = y1(:,1); <span class="comment">%actual position</span>Vtrue = y1(:,2); <span class="comment">%actiual velocity</span>z = y2; <span class="comment">%measured position</span>X = y3; <span class="comment">%Kalman filter output</span>t = t*dt; <span class="comment">% actual time</span><span class="comment">%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%</span><span class="comment">%%% Position analysis %%%%%%%%%%%%%%%%%%</span><span class="comment">%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%</span>figure;subplot(211)plot(t,Xtrue,<span class="string">'g'</span>,t,z,<span class="string">'c'</span>,t,X(:,1),<span class="string">'m'</span>,<span class="string">'linewidth'</span>,2);title(<span class="string">'Position estimation results'</span>);xlabel(<span class="string">'Time (s)'</span>);ylabel(<span class="string">'Position (m)'</span>);legend(<span class="string">'True position'</span>,<span class="string">'Measurements'</span>,<span class="string">'Kalman estimated displacement'</span>,<span class="string">'Location'</span>,<span class="string">'NorthWest'</span>);<span class="comment">%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%</span><span class="comment">%%% Velocity analysis %%%%%%%%%%%%%%%%%%</span><span class="comment">%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%</span><span class="comment">% The instantaneous velocity as derived from 2 consecutive position</span><span class="comment">% measurements</span>InstantV = [10;diff(z)/dt];<span class="comment">% The instantaneous velocity as derived from running average with a window</span><span class="comment">% of 5 samples from instantaneous velocity</span>WindowSize = 5;InstantVAverage = filter(ones(1,WindowSize)/WindowSize,1,InstantV);<span class="comment">% figure;</span>subplot(212)plot(t,InstantV,<span class="string">'g'</span>,t,InstantVAverage,<span class="string">'c'</span>,t,Vtrue,<span class="string">'m'</span>,t,X(:,2),<span class="string">'k'</span>,<span class="string">'linewidth'</span>,2);title(<span class="string">'Velocity estimation results'</span>);xlabel(<span class="string">'Time (s)'</span>);ylabel(<span class="string">'Velocity (m/s)'</span>);legend(<span class="string">'Estimated velocity by raw consecutive samples'</span>,<span class="string">'Estimated velocity by running average'</span>,<span class="string">'True velocity'</span>,<span class="string">'Estimated velocity by Kalman filter'</span>,<span class="string">'Location'</span>,<span class="string">'NorthWest'</span>);set(gcf,<span class="string">'Position'</span>, [100 100 600 800]);</pre><img vspace="5" hspace="5" src="feedbackviewKF_02.png"> <h2>Example 3: A 2-input 2-output 4-state system<a name="6"></a></h2><pre class="codeinput">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 0;0 0];<span class="comment">% process noise variance</span>Q=diag([0.5^2 0.2^2 0.3^2 0.5^2]);<span class="comment">% measurment noise variance</span>R=eye(2);<span class="comment">% random initial state</span>x0 = randn(4,1);<span class="comment">% Kalman filter set up</span><span class="comment">% The same model</span>A1 =A;B1 = B;C1 = C;D1 = D;Q1 = Q;R1 = R;<span class="comment">% However, zeros initial state</span>x1 = zeros(4,1);<span class="comment">% Initial state covariance</span>P1 = 10*eye(4);<span class="comment">% Simulation set up</span><span class="comment">% time span 100 samples</span>tspan = [0 1000];<span class="comment">% random input change every 100 samples</span>u = [(0:100:1000)' randn(11,2)];<span class="comment">% simulation</span>[t,x,y1,y2,y3,y4] = sim(<span class="string">'feedbackkf'</span>,tspan,[],u);<span class="comment">% plot results</span><span class="comment">% Display results</span>t = t*dt;figureset(gcf,<span class="string">'Position'</span>,[100 100 600 800])<span class="keyword">for</span> k=1:4 subplot(4,1,k) plot(t,y1(:,k),<span class="string">'b'</span>,t,y3(:,k),<span class="string">'r'</span>,<span class="string">'linewidth'</span>,2); legend(<span class="string">'Actual state'</span>,<span class="string">'Estimated state'</span>,<span class="string">'Location'</span>,<span class="string">'best'</span>) title(sprintf(<span class="string">'state %i'</span>,k))<span class="keyword">end</span>xlabel(<span class="string">'time, s'</span>)</pre><img vspace="5" hspace="5" src="feedbackviewKF_03.png"> <h2>Constant Kalman Filter Gain<a name="7"></a></h2> <p>The time varying Kalman filter gain actually represents a discrete-time Riccati equation. Its steady-state solution can be obtained by using the KALMD function in the Control Systems Toolbox. We also can run the Kalman filter gain model for a sufficiently long time to get an approximate steady-state solution. </p> <p>For example, consider the 2-input 2 output and 4-state example above. Run the KFgain model for 1000 iterations.</p><pre class="codeinput">[t,x,y]=sim(<span class="string">'KFgain'</span>,[0 1000]);<span class="comment">%The steady-state gain is</span>K=y(:,:,end);disp(K)</pre><pre class="codeoutput"> 0.1048 0.2426 -0.1235 -0.0168 0.0076 0.0758 -0.1843 0.2888</pre><h2>Time-Invariant Kalman Filter<a name="8"></a></h2> <p>With the steady-state gain, the Kalman filter can be simplified as a linear time-invariant (LTI) system.</p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq152588.png"> </p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq120013.png"> </p> <p><img vspace="5" hspace="5" src="feedbackviewKF_eq125477.png"> </p> <p>This system has been implemented in the Simulink model KalmanLTI:</p> <p><img vspace="5" hspace="5" src="KalmanLTI.png"> </p> <p>With this model, the simulation results of the 2-input, 2-output and 4-state example are as follwos.</p><pre class="codeinput">[t,x,y1,y2,y3,y4] = sim(<span class="string">'KalmanLTI'</span>,tspan,[],u);<span class="comment">% plot results</span><span class="comment">% Display results</span>t = t*dt;figureset(gcf,<span class="string">'Position'</span>,[100 100 600 800])<span class="keyword">for</span> k=1:4 subplot(4,1,k) plot(t,y1(:,k),<span class="string">'b'</span>,t,y3(:,k),<span class="string">'r'</span>,<span class="string">'linewidth'</span>,2); legend(<span class="string">'Actual state'</span>,<span class="string">'Estimated state'</span>,<span class="string">'Location'</span>,<span class="string">'best'</span>) title(sprintf(<span class="string">'state %i'</span>,k))<span class="keyword">end</span>⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -