本節卡爾曼濾波Matlab實現是針對線性系統估計的，僅為簡單模擬。

1.離散時間線性動態系統的狀態方程

  線性系統採用狀態方程、觀測方程及其初始條件來描述。線性離散時間系統的一般狀態方程可描述為

  其中，X(k) 是 k 時刻目標的狀態向量，V(k)是過程噪聲，它是具有均值為零、方差矩陣為 Q(k) 的高斯噪聲向量，即
　

  Q(k)是狀態轉移矩陣， G(k)是過程噪聲增益矩陣。

2.感測器的測量（觀測）方程

  感測器的通用觀測方程為

　
  這裡， Z(k+1)是感測器在 k+1 時刻的觀測向量，觀測噪聲 W(k+1) 是具有零均值和正定協方差矩陣 R(k+1) 的高斯分佈測量噪聲向量，即

3.初始狀態的描述

  初始狀態 X(0) 是高斯的，具有均值 X(0|0) 和協方差 ，即

4.Kalman濾波演算法

  狀態估計的一步預測方程為

  一步預測的協方差為

  預測的觀測向量為

  觀測向量的預測誤差協方差為

  新息或量測殘差為

  濾波器增益為

  Kalman濾波演算法的狀態更新方程為

  濾波誤差協方差的更新方程為

% Kalman濾波技術

A=1;                                        % 狀態轉移矩陣 Φ(k)
H=0.2;                                      % 觀測矩陣 H(k)
 

X(1)=0;                                     % 目標的狀態向量 X(k)
% V(1)=0;                                   % 過程噪聲 V(k)
Y(1)=1;                                     % 一步預測x(k)的更新 X(k+1|k+1)
P(1)=10;                                    % 一步預測的協方差 P(k)
N=200;
V=randn(1,N);                               % 模擬產生過程噪聲(高斯分佈的隨機噪聲)
 

w=randn(1,N);                               % 模擬產生測量噪聲

for k=2:N

    X(k) = A * X(k-1)+V(k-1);               % 狀態方程:X(k+1)=Φ(k)X(k)+G(k)V(k),其中G(k)=1

end

Q=std(V)^2;                                 % W(k)的協方差,std()函式用於計算標準偏差  
R=std(w)^2;                                 % V(k)的協方差 covariance

Z=H*X+w;                                    % 觀測方程:Z(k+1)=H(k+1)X(k+1)+W(k+1),Z(k+1)是k+1時刻的觀測值

for t=2:N

    P(t) = A * P(t-1)+Q;                    % 一步預測的協方差 P(k+1|k)   

    S(t) = H.^2 * P(t)+R;                   % 觀測向量的預測誤差協方差 S(k+1)

    K(t) = H * P(t)/S(t);                   % 卡爾曼濾波器增益 K(k+1) 

    v(t) = Z(t) - ( A * H * Y(t-1) );       % 新息/量測殘差 v(k+1)

    Y(t)=A * Y(t-1) + K(t) * v(t);          % 狀態更新方程 X(k+1|k+1)=X(k+1|k)+K(k+1)*v(k+1)

    P(t)=(1-H * K(t)) * P(t);               % 誤差協方差的更新方程: P(k+1|k+1)=(I-K(k+1)*H(k+1))*P(k+1|k)
end


t=1:N;
plot(t,Y,'r',t,Z,'g',t,X,'b');              % 紅色線最優化估算結果濾波後的值，%綠色線觀測值，藍色線預測值
legend('Kalman濾波結果','觀測值','預測值');

執行結果