卡爾曼濾波器是一個 ”optimal recursive data processing algorithm”（最優化自迴歸資料處理演算法）

先說一個例子： 假設我們要研究的物件是一個房間的溫度。

1.根據經驗，溫度是恆定的，即上一分鐘的溫度等於現在這一分鐘的溫度，經驗即預測，但這並不是完全可信的，即存在一定的誤差。我們假設成高斯白噪聲。 2.另外在房間裡放一個溫度計，實時檢測房間的溫度，這是觀測值。同樣地也存在誤差，也是高斯白噪聲。

我們現在的目標就是，用這兩個值，結合它們各自的噪聲，估算出房間的實際溫度。假設要估算k時刻的實際溫度值，首先需要知道k-1時刻的溫度值。

1)預測值，假設k-1時刻房間溫度為23度，則k時刻的溫度應該也為23度（恆定）。同時該值的偏差為5（5是3的平方加上4的平方再開方，其中3為k-1時刻估算出的最優溫度值的偏差3，4為對預測的偏差）。

2)測量值，假設為25度，同時偏差為4度。

那麼實際值為多少？相信誰多一點，可用它們的協方差來判斷： ，Kg = 0.78

則實際的溫度值是：23+0.78*(25-23) = 24.56度。可以看出溫度計測量的covariance較小，所以實際值比較偏向溫度計。

在進入k+1時刻估算前，需要算出k時刻最優值（24.56）的偏差。演算法，5即k時刻估算所用k-1時刻23度溫度值的偏差。得出的2.35即k時刻最優值的偏差（對應上面k-1時刻的3）。

這樣卡爾曼濾波器就不斷得把covariance遞迴，從而估算出最優值。而且它只保留上一刻的covariance。上面的Kg就是卡爾曼增益（Kalman Gain）。

真正工程系統上的卡爾曼濾波器演算法 首先引入一個離散控制過程的系統，改系統可用一個線性隨機微分方程（linear stochastic difference equation）來描述：

再加上系統的測量值：

其中：X(k)是k時刻的系統狀態，U(k)是k時刻對系統的控制量。A和B是系統引數，對於多模系統，它們為矩陣。

Z(k)是k時刻的測量值，H是測量引數，對於多模系統，H為矩陣。W(k)和V(k)分別表示過程和測量的噪聲，它們被假設成高斯白噪聲，它們的covariance分別是Q，R。

對於滿足上面條件（線性隨機微分系統，過程和測量都是高斯白噪聲），卡爾曼濾波器是最優的資訊處理器。

卡爾曼濾波器演算法流程及核心公式

X(k | k-1) 是利用上一狀態預測的結果，X(k-1 | k-1) 是上一狀態最優結果。U(k)為現在狀態的控制量，若沒有，可為0.

現在更新 X(k | k-1) 的covariance，用P表示：

其中 P(k | k-1) 是 X(k | k-1) 對應的covariance；P(k-1 | k-1) 是 X(k-1 | k-1) 對應的covariance。A’ 表示A的轉置矩陣，Q是系統的covariance。

式(1)(2)是5各公式的前兩個，用來對系統進行預測。

然後再收集系統的觀測值 Z(k)，則最優值：

其中Kg為卡爾曼增益（Kalman Gain）:

到目前為止，已經得到k狀態下最優的估算值 X(k | k-1)。

但是為了要卡爾曼濾波器不斷得執行下去直到系統過程結束，需要更新k狀態下 X(k | k-1) 的covariance：

其中I為1的矩陣，對於單模系統I = 1。

當系統進入k+1狀態時，P(k | k) 就是式(2)中的 P(k-1 | k-1)。這樣，演算法就可以自迴歸地運算下去。

擴充套件卡爾曼濾波器 由上述的介紹可以得知，卡爾曼濾波器只適用於線性系統模型，然而實際中的系統往往都是非線性模型。所示必須對卡爾曼濾波器進行修改。

首先要了解一下線性化卡爾曼濾波。它和線性卡爾曼濾波器在濾波器的演算法方面有同樣的演算法結構。不一樣的地方在於這兩者的系統模型不同。線性卡爾曼濾波器的系統本身就是線性系統，而線性化卡爾曼濾波器的系統本身是非線性系統，但是機智的大神們將非線性的系統進行了線性化，於是卡爾曼濾波就可以用在非線性系統中了。對於一個卡爾曼濾波器的設計者，就可以不管模型到底是一開始就是線性系統還是非線性系統線性化得到的線性系統，反正只要是線性系統就好了。

但是線性化卡爾曼濾波器會發散。為什麼會發散呢？是這樣，我們在對非線性系統進行線性化的過程中，只有被線性化的那個點附近的線性化模型和真實的模型相近，遠的誤差就大了，那麼這個時候卡爾曼濾波器的效果就不好。所以線性化的這個限制要時刻考慮，這也就是為什麼要把線性卡爾曼濾波器和線性化卡爾曼濾波器區分開的理由。而決定一個線性化濾波器成功與否的關鍵就在於這個濾波器系統模型線性化得好不好。

EKF基於線性化系統的思想，將系統函式的非線性函式作一階Taylor展開，得到線性化系統方程。擴充套件的卡爾曼濾波器演算法就是適用於非線性系統的卡爾曼濾波器。它與經典的線性卡爾曼濾波器很相似，演算法步驟和結構都相同。不同在於系統模型和矩陣A和H。

MATLAB實現卡爾曼濾波器（房間溫度例子）

% Kalman filter example of temperature measurement in Matlab implementation of Kalman filter algorithm.
% 房間當前溫度真實值為24度，認為下一時刻與當前時刻溫度相同，誤差為0.02度（即認為連續的兩個時刻最多變化0.02度）。
% 溫度計的測量誤差為0.5度。
% 開始時，房間溫度的估計為23.5度，誤差為1度。
close all;
 
% intial parameters
% 計算連續n_iter個時刻
n_iter = 100;
% size of array. n_iter行，1列
sz = [n_iter, 1];
% 溫度的真實值
x = 24;
% 過程方差，反應連續兩個時刻溫度方差。更改檢視效果
Q = 4e-4;
% 測量方差，反應溫度計的測量精度。更改檢視效果
R = 0.25;
% z是溫度計的測量結果，在真實值的基礎上加上了方差為0.25的高斯噪聲。
z = x + sqrt(R)*randn(sz);
% 對陣列進行初始化
% 對溫度的後驗估計。即在k時刻，結合溫度計當前測量值與k-1時刻先驗估計，得到的最終估計值
xhat = zeros(sz); 
% 後驗估計的方差
P = zeros(sz); 
% 溫度的先驗估計。即在k-1時刻，對k時刻溫度做出的估計
xhatminus = zeros(sz);
% 先驗估計的方差
Pminus = zeros(sz);
% 卡爾曼增益，反應了溫度計測量結果與過程模型（即當前時刻與下一時刻溫度相同這一模型）的可信程度
K = zeros(sz); 
% intial guesses
xhat(1) = 23.5; %溫度初始估計值為23.5度
P(1) =1; % 誤差方差為1
 
for k = 2:n_iter
    % 時間更新（預測）
    % 用上一時刻的最優估計值來作為對當前時刻的溫度的預測
    xhatminus(k) = xhat(k-1);
    % 預測的方差為上一時刻溫度最優估計值的方差與過程方差之和
    Pminus(k) = P(k-1)+Q;
    % 測量更新（校正）
    % 計算卡爾曼增益
    K(k) = Pminus(k)/( Pminus(k)+R );
    % 結合當前時刻溫度計的測量值，對上一時刻的預測進行校正，得到校正後的最優估計。該估計具有最小均方差
    xhat(k) = xhatminus(k)+K(k)*(z(k)-xhatminus(k));
    % 計算最終估計值的方差
    P(k) = (1-K(k))*Pminus(k);
end
 
FontSize = 14;
LineWidth = 3;
figure();
plot(z,'k+'); %畫出溫度計的測量值
hold on;
plot(xhat,'b-','LineWidth',LineWidth) %畫出最優估計值
hold on;
plot(x*ones(sz),'g-','LineWidth',LineWidth); %畫出真實值
legend('溫度計的測量結果', '後驗估計', '真實值');
xl = xlabel('時間(分鐘)');
yl = ylabel('溫度');
set(xl,'fontsize',FontSize);
set(yl,'fontsize',FontSize);
hold off;
set(gca,'FontSize',FontSize);
 
figure();
valid_iter = 2:n_iter; % Pminus not valid at step 1
% 畫出最優估計值的方差
plot(valid_iter,P(valid_iter),'LineWidth',LineWidth);
legend('後驗估計的誤差估計');
xl = xlabel('時間(分鐘)');
yl = ylabel('℃^2');
set(xl,'fontsize',FontSize);
set(yl,'fontsize',FontSize);
set(gca,'FontSize',FontSize);

MATLAB實現擴充套件卡爾曼濾波器

% function: simulating the process of EKF
N = 50;           % 計算連續N個時刻
n = 3;             % 狀態維度
q = 0.1;          % 過程標準差
r = 0.2;           % 測量標準差
% eye函式產生單位矩陣
Q = q^2*eye(n);   % 過程方差
R = r^2;               % 測量值的方差
 
%{
     FUNHANDLE = @FUNCTION_NAME returns a handle to the named function,
     FUNCTION_NAME. A function handle is a MATLAB value that provides a
     means of calling a function indirectly. 
%}
f = @(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))];  % 狀態方程
h = @(x)[x(1);x(2);x(3)];                          % 測量方程
s = [0;0;1];                                            % 初始狀態
 
% 初始化狀態
x = s+q*randn(3,1); 
% eye返回單位矩陣
P = eye(n);
% 3行50列，一列代表一個數據
% 最優估計值
xV = zeros(n,N);
% 真實值
sV = zeros(n,N);
% 狀態測量值
zV = zeros(n,N);
 
for k = 1:N
    z = h(s) + r * randn;
    % 實際狀態
    sV(:,k) = s;
    % 狀態測量值
    zV(:,k) = z;
    
    % 計算f的雅可比矩陣，其中x1對應黃金公式line2
    [x1,A] = jaccsd(f,x);
    % 過程方差預測，對應line3
    P = A*P*A'+Q;
    % 計算h的雅可比矩陣
    [z1,H] = jaccsd(h,x1);
    
    % 卡爾曼增益，對應line4
    % inv返回逆矩陣
    K = P*H'*inv(H*P*H'+R);
    % 狀態EKF估計值，對應line5
    x = x1+K*(z-z1);
    % EKF方差，對應line6
    P = P-K*H*P;
    
    % save
    xV(:,k) = x;
    % update process 
    s = f(s) + q*randn(3,1);
end
for k = 1:3
    FontSize = 14;
    LineWidth = 1;
    
    figure();
    % 畫出真實值
    plot(sV(k,:),'g-');
    hold on;
    
    % 畫出最優估計值
    plot(xV(k,:),'b-','LineWidth',LineWidth);
    hold on;
    
    % 畫出狀態測量值
    plot(zV(k,:),'k+');
    hold on;
    
    legend('真實狀態', 'EKF最優估計估計值','狀態測量值');
    xl = xlabel('時間(分鐘)');
    % 把數值轉換成字串， 轉換後可以使用fprintf或disp函式進行輸出。
    t = ['狀態 ',num2str(k)] ;
    yl = ylabel(t);
    set(xl,'fontsize',FontSize);
    set(yl,'fontsize',FontSize);
    hold off;
    set(gca,'FontSize',FontSize);
end