卡爾曼濾波是一種高效率的遞歸濾波器，它能夠從一系列的不完全及包含噪聲的測量中，估計動態系統的狀態。卡爾曼濾波在技術領域有許多的應用，常見的有飛機及太空船的導引、導航及控制。
卡爾曼算法主要可以分為兩個步驟進行：預測和更新。基於最小均方誤差為最佳估計准則，利用上一時刻的估計值和狀態轉移矩陣進行預測，用測量值對預測值進行修正，得到當前時刻的估計值。
卡爾曼算法公式
預測：
-
\(\hat s(n|n-1)=A\hat s(n-1|n-1)\)
-
\(P(n)=A\xi(n-1)A^T+Q\)
更新：
-
\(G(n)=P(n)C^T[CP(n)C^T+R]^{-1}\)
-
\(\xi(n)=(I-G(n)C)P(n)\)
-
\(\hat s(n|n)=\hat s(n|n-1)+G(n)[x(n)-C\hat s(n|n-1)]\)
利用上面五個式子可以遞推得到狀態的估計值\(\hat s(n|n)\)。
文章的組織如下：
1.基本模型及假設
2.卡爾曼算法原理及推導
3.卡爾曼濾波算法舉例
4.Matlab程序
1.基本模型與假設
狀態方程（描述物體運動狀態）
測量方程（利用探測器等器件獲取物體狀態參數）
其中\(w(n)\)為過程噪聲，\(v(n)\)為測量噪聲。
假設：
\(w(n)\)，\(v(n)\)，為獨立零均值的白噪聲過程，即
\(v(n)\)和\(s(n)\)、\(w(n)\)不相關，即
2.卡爾曼算法原理及推導
基於最小均方誤差准則，通過觀測值\(x(n)\)求真實信號\(s(n)\)的線性無偏最優估計。
已知上一時刻的估計值\(\hat s(n-1|n-1)\)
利用狀態方程對\(s(n)\)進行預測，最佳預測為
利用測量方程對\(x(n)\)進行預測，最佳預測為
噪聲不參與預測。
當n時刻到來時，已知\(x(n)\)，可以得到預測誤差（新息）
選擇合適的系數\(G(n)\)對預測誤差進行加權，作為對預測值\(\hat s(n|n-1)\)的修正值。修正后得到對信號的最佳估計為
其中\(G(n)\)是未知的，因此要求出\(G(n)\)的關系式。
最佳估計使得均方誤差最小，即
求\(\xi(n)\)對\(G(n)\)的偏導數並令其等於零，有
對公式中的兩項分別進行變換，有
令一步預測誤差
預測誤差功率
有
則有
由上式可解得
\(E[e(n)x^T(n)]=0\)，\(E[e(n)\hat s(n|n-1)]=0\)，有
有
均方誤差
將其展開並且\(v(n)\)和\(e_1(n)\)不相關，有
可以看出，通過對信號進行修正，最小均方誤差比預測誤差功率小\(G(n)CP(n)\)
其中
要求\(E[e(n)w^T(n)]=0\)
即所有公式都得到。
預測：
-
\(\hat s(n|n-1)=A\hat s(n-1|n-1)\)
-
\(P(n)=A\xi(n-1)A^T+Q\)
更新：
-
\(G(n)=P(n)C^T[CP(n)C^T+R]^{-1}\)
-
\(\xi(n)=(I-G(n)C)P(n)\)
-
\(\hat s(n|n)=\hat s(n|n-1)+G(n)[x(n)-C\hat s(n|n-1)]\)
設定初始值\(\hat s(n-1|n-1)\)、\(\xi(n-1)\)，（可以設為0）
A，C，Q，R通過測量方程、狀態方程以及噪聲方差得到。
3.卡爾曼濾波算法應用
假設有一勻速轉動模型，角度量為\(\alpha\)，角速度為\(\omega\)
由勻速運動的運動學方程可以得到
假定狀態量為\(s(n)=\begin{bmatrix} \alpha(n)\\ \omega(n) \end{bmatrix}\)
由運動學方程可以得到狀態方程
假設過程噪聲方差為\(\sigma_1=0.1\)，則\(Q=\begin{bmatrix}0&0\\ 0&\sigma_1 \end{bmatrix}\)
測量值可以看作狀態值加上一個測量噪聲得到
測量方程
假設測量噪聲方差為\(\sigma_a=1\)，則\(R=\sigma_a\)
設定初始角度為10，角速度為2。
初始預測值設為0，即\(\hat s(0|0)=0\)，\(\xi(0)=\begin{bmatrix}0&0\\ 0&0\end{bmatrix}\)
其中藍色直線表示的是真實軌跡，可以看到測量值相對於真實軌跡有很大的噪聲，Kalman算法收斂后可以比較好的貼近真實值。
4.Matlab代碼
clc;clear all;
T = 0.01; %采樣間隔
sigma_1 = 0.1;
sigma_a = 1;
N = 200;
A = [1 T; 0 1];
C = [1 0];
Q = [0 0; 0 sigma_1];
R = sigma_a;
%v = sqrt(sigma_a)*randn(1,N);
%save('v01','v')
load('v01');
s = zeros(2,N);
s_estimate = zeros(2,N);
x = zeros(1,N);
xi = zeros(2);
%給定初始值
s(:,1) = [10 2]';
s_estimate(:,1) = [0 0];
%% Kalman 算法
for n=2:N
s(:,n) = A*s(:,n-1); %狀態方程
x(:,n) = C*s(:,n)+v(n); %測量方程
%kalman濾波算法
P = A*xi*A'+Q;
G = P*C'*(C*P*C'+R)^(-1);
s_estimate(:,n) = A*s_estimate(:,n-1)+G*(x(:,n)-C*A*s_estimate(:,n-1));
xi = P-G*C*P;
end
t = 1:200;
figure;
plot(t,s(1,:),t,x,t,s_estimate(1,:));
legend('真實軌跡','測量值','kalman估計值');
xlabel('取樣點數/ ×0.01s');
ylabel('幅值');
error1 = s(1,:)-x;
error2 = s(1,:)-s_estimate(1,:);
figure;
plot(t,error1,t,error2);
xlabel('取樣點數/ ×0.01s');
ylabel('幅值');
legend('測量值-真實值','估計值-真實值');
title(['過程噪聲方差為',num2str(sigma_1)]);
這是我學習Kalman算法時整理的內容，也算是一個總結，供大家參考。如有不當之處，還請多多見諒。