本文主要內容借鑑http://blog.csdn.net/heyijia0327/article/details/17487467博客當中的內容。
有一個勻加速運動的小車，它受到的合力爲 ft , 由勻加速運動的位移和速度公式，能得到由 t-1 到 t 時刻的位移和速度變化公式：
該系統系統的狀態向量包括位移和速度，分別用 xt 和 xt的導數 表示。控制輸入變量爲u，也就是加速度，於是有如下形式:
所以這個系統的狀態的方程爲：
這裏對應的的矩陣A大小爲 2*2 ,矩陣B大小爲 2*1。
貌似有了這個模型就能完全估計系統狀態了，速度能計算出，位移也能計算出。那還要卡爾曼幹嘛，問題是很多實際系統複雜到根本就建不了模。並且，即使你建立了較爲準確的模型，只要你在某一步有誤差，由遞推公式，很可能不斷將你的誤差放大A倍（A就是那個狀態轉換矩陣），以至於最後得到的估計結果完全不能用了。回到最開始的那個笑話，如果那個完全憑預測的特種兵在某一步偏離了正確的路徑，當他站在錯誤的路徑上（而他自己以爲是正確的）做下一步預測時，肯定走的路徑也會錯了，到最後越走越偏。
既然如此，我們就引進反饋。從概率論貝葉斯模型的觀點來看前面預測的結果就是先驗，測量出的結果就是後驗。
測量值當然是由系統狀態變量映射出來的，方程形式如下：
注意Z是測量值，大小爲m*1(不是n*1，也不是1*1，後面將說明），H也是狀態變量到測量的轉換矩陣。大小爲m*n。隨機變量v是測量噪聲。
同時對於勻加速模型，假設下車是勻加速遠離我們，我們站在原點用超聲波儀器測量小車離我們的距離。
也就是測量值直接等於位移。可能又會問，爲什麼不直接用測量值呢？測量值噪聲太大了，根本不能直接用它來進行計算。試想一個本來是朝着一個方向做勻加速運動的小車，你測出來的位移確是前後移動（噪聲影響），只根據測量的結果，你就以爲車子一會往前開一會往後開。
對於狀態方程中的系統噪聲w和測量噪聲v，假設服從如下多元高斯分佈，並且w,v是相互獨立的。其中Q,R爲噪聲變量的協方差矩陣。
看到這裏自然要提個問題，爲什麼噪聲模型就得服從高斯分佈呢？請繼續往下看。
對於小車勻加速運動的的模型，假設系統的噪聲向量只存在速度分量上，且速度噪聲的方差是一個常量0.01，位移分量上的系統噪聲爲0。測量值只有位移，它的協方差矩陣大小是1*1，就是測量噪聲的方差本身。
那麼：
Q中，疊加在速度上系統噪聲方差爲0.01，位移上的爲0，它們間協方差爲0，即噪聲間沒有關聯。
使用卡爾曼濾波器估計小車勻加速運動的
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%日 期： 2015.10.12
%程序功能： 使用卡爾曼濾波器估計小車勻加速運動
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all
% 初始化參數
delta_t=0.1; %採樣時間
t=0:delta_t:5;
N = length(t); % 序列的長度
sz = [2,N]; % 信號需開闢的內存空間大小 2行*N列 2:爲狀態向量的維數n
g=10; %加速度值
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x=1/2*g*t.^2; %實際真實位置
z = x + sqrt(10).*randn(1,N); % 測量時加入測量白噪聲
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q =[0 0;0 9e-1]; %假設建立的模型 噪聲方差疊加在速度上 大小爲n*n方陣 n=狀態向量的維數
R = 10; % 位置測量方差估計，可以改變它來看不同效果 m*m m=z(i)的維數
A=[1 delta_t;0 1]; % n*n
B=[1/2*delta_t^2;delta_t];
H=[1,0]; % m*n
n=size(Q); %n爲一個1*2的向量 Q爲方陣
m=size(R);
% 分配空間
xhat=zeros(sz); % x的後驗估計
P=zeros(n); % 後驗方差估計 n*n
xhatminus=zeros(sz); % x的先驗估計
Pminus=zeros(n); % n*n
K=zeros(n(1),m(1)); % Kalman增益 n*m
I=eye(n);
% 估計的初始值都爲默認的0，即P=[0 0;0 0],xhat=0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%EKF過程
for k = 9:N %假設車子已經運動9個delta_T了，我們纔開始估計
% 時間更新過程
xhatminus(:,k) = A*xhat(:,k-1)+B*g;
Pminus= A*P*A'+Q;
% 測量更新過程
K = Pminus*H'*inv( H*Pminus*H'+R );
xhat(:,k) = xhatminus(:,k)+K*(z(k)-H*xhatminus(:,k));
P = (I-K*H)*Pminus;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(1)
plot(t,z);
hold on
plot(t,xhat(1,:),'r-')
plot(t,x(1,:),'g-');
legend('含有噪聲的測量', '後驗估計', '真值');
xlabel('Iteration');
%% Estimate error(估計誤差)
x_error = x-xhat(1,:);
%% Graph 2
figure(2)
plot(t,x_error(1,:)),grid on;
title('位置誤差')
xlabel('時間 [sec]')
ylabel('位置均方根誤差 [m]')
hold off;
%% Graph 3
figure(3)
plot(t,xhat(2,:),'r'),grid on;
title('實際速度 ')
legend('實際速度')
xlabel('時間 [sec]')
ylabel('速度 [m/sec]')
hold off;
%==========================================================================
