文章目錄

• 0.引言
• 1.場景預設
• 2.卡爾曼濾波器
• 3.仿真及效果

0.引言

\qquad 本文參考了Matlab對卡爾曼濾波器的官方教程及幫助文檔（Kalman Filter）。官方教程的B站鏈接如下，在此對分享資源的Up主表示感謝。(如不能正常播放或需要看中文字幕，請點擊此處B站鏈接)

【官方教程】卡爾曼濾波器教程與MATLAB仿真（全)(中英字幕)

另外提供友情鏈接如下：

卡爾曼濾波算法（知乎，多維情形）

Matlab關于傳感器融合知識的介紹

擴展卡爾曼濾波算法（CSDN，適合入門）

擴展卡爾曼濾波算法（知乎，適合入門）

擴展卡爾曼濾波算法（知乎，適合進階）

\qquad本文不會完全照搬視頻中的所有內容，只會介紹有關卡爾曼濾波器關于定位方面的知識。卡爾曼濾波器除最原始的版本（KF）外，其延伸版本主要有三種——擴展卡爾曼濾波器（EKF）、無跡卡爾曼濾波器（UKF）、粒子濾波器（PF）。它們的運算復雜度依次遞增，其中KF、EKF、UKF是建立在隨機噪聲是高斯分布的基礎上的；PF理論則沒有此預設。它們的關系如下圖所示：

\qquad首先，KF是一種狀態觀測器。狀態觀測器是針對可觀系統，根據輸出$y$和輸入$u$對系統內部狀態$u$進行觀測的結構單元。其構圖如下（參考視頻鏈接）：

原系統方程為：
$$\{\begin{array}{l}\dot{x}=Ax+Bu\\ y=Cx\end{array}$$
采用狀態觀測器的觀測系統方程為：
$$\{\begin{array}{l}\dot{\hat{x}}=A\hat{x}+Bu+K(y?\hat{y})\\ \hat{y}=C\hat{x}\end{array}$$
為保證觀測器的$$\underset{t\to \infty }{\lim }(x?\hat{x}{)}^T(x?\hat{x})=0$$
需要$A?KC$是負定的。而卡爾曼濾波器就是一種狀態觀測器，只不過它是隨機系統的狀態觀測器，其結構框圖如下：

在輸入$u$和動態系統Plant中間會引入過程噪聲$w$，而在輸出$y$（即測量）和實際測量$y_v$中間會引入傳感器噪聲$v$,而卡爾曼濾波器則是根據$u,y_v$求得測量的最優估計$y_e$。

1.場景預設

\qquad 假設一輛汽車在做勻速直線運動（一維場景），駕駛員通過油門控制了汽車的加速度恒定。忽略汽車所受的阻力、質量變化，并假設駕駛員操作會給汽車的牽引力造成一定的過程噪聲。選擇汽車的位移和速度為狀態變量$x=[p,v{]}^T$，則狀態變量導數為汽車的速度和加速度即$\dot{x}=[v,a{]}^T$，選取控制變量$u=a$，測量量為汽車的位移。則狀態方程如下：
$$?\begin{array}{l}\dot{x}=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]x+\left[\begin{array}{c}0\\ 1\end{array}\right]u\\ y=\left[\begin{array}{cc}0& 1\end{array}\right]x\end{array}$$
選取采用間隔$dt$，則狀態方程離散化后變為：
$$?\begin{array}{l}x(n)=\left[\begin{array}{cc}1& dt\\ 0& 1\end{array}\right]x(n?1)+\left[\begin{array}{c}0\\ dt\end{array}\right]u(n?1)\\ y(n)=\left[\begin{array}{cc}0& 1\end{array}\right]x(n)\end{array}$$
按照慣例定義
$$A=\left[\begin{array}{cc}1& dt\\ 0& 1\end{array}\right],B=\left[\begin{array}{c}0\\ dt\end{array}\right],C=\left[\begin{array}{cc}0& 1\end{array}\right],I=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$$
由于系統具有一定的過程噪聲$w$和測量噪聲$v$。引入噪聲的離散系統狀態方程為：
$$\{\begin{array}{l}x(n)=Ax(n?1)+B(u(n?1)+w^{\prime })=Bu(n)+w\\ y_v(n)=Cx(n)+v\end{array}$$
為加以區別，使用$y_v$在本文中表示含噪聲的真實測量。定義隨機噪聲的方差：
$E((w?\overline{w}{)}^T(w?\overline{w}))=Q,E((v?\overline{v}{)}^T(v?\overline{v}))=R$
汽車的初始真實位移為0.2，真實速度為0即$x_0=[0.2,0{]}^T$.

2.卡爾曼濾波器

\qquad為方便初學者入門，本文不會從貝葉斯估計的角度證明KF的公式，但會將其應用步驟以盡可能簡單的手段列出。

需要設定一個初始的狀態估計值即${\hat{x}}_0$，需要認為規定初始的先驗估計的協方差矩陣$P_0=E((x?\hat{x}{)}^T(x?\hat{x}))$。${\hat{x}}_0$一般是憑經驗設定的，如果系統偏離平衡狀態不遠設定$[0,0{]}^T$也無妨；$P_0$在教學視頻中并未給出設定方式，一般設定為$Q$即可。令$k=1$，仿真開始。

獲取當前測量量$y_v$，先求出$x,P$的先驗估計，即$$\begin{gathered} {\hat{x}}_k^{?}=A{\hat{x}}_{k?1}+B{u}_k \\ {P}_k^{?}=A{P}_{k?1}{A}^T+Q \end{gathered}$$

根據$x,P$的先驗估計，求出$x$的后驗估計$\hat{x}$，即$$\begin{gathered} K_k=\frac{P_k^{?}{C}^T}{C{P}_k^{?}{C}^T+R} \\ {\hat{x}}_k={\hat{x}}_k^{?}+K_k(y_v?C{\hat{x}}_k^{?}) \\ {P}_k=(I?K_{k}C)P_k^{?} \end{gathered}$$

根據$\hat{y}=C\hat{x}$求出測量的卡爾曼估計值$\hat{y}$.

$k=k+1$,轉步2

\qquad嚴格地來說，如果步驟4要成立，需要$E((w?\overline{w}{)}^T(x?\overline{x}))=0$即$w$和$v$下相互獨立，且系統方程中直接傳輸矩陣$D=0$，但考慮到本文是寫給初學者的，所以在此默認了兩個條件是成立的。
\qquad狀態協方差即為P，測量協方差為$CP{C}^T$。卡爾曼濾波器算法收斂一般是指的是測量協方差收斂。

3.仿真及效果

在此附上Matlab的仿真代碼

% 模擬要求汽車在一維空間的加速和減速過程 % 控制變量u是汽車的加速度 % 狀態變量x=[p,v],x^hat=[v,a] % w為控制變量的隨機擾動，v為測量的隨機擾動 % Q為w的方差，R為v的方差，假設w與v相互獨立 clear dt = 0.1; % 采樣間隔 N = 100; % 仿真數 Q = diag([0,0.05]); R = 1; A = [1,dt;0,1]; B = [0;dt]; C = [1,0]; P = Q; I = eye(2); x0 = [0.2;0]; % 初始位置和速度 xh0 = [0;0]; % x0的估計 u = ones(1,N); % 加速度恒定 w = sqrt(Q)*randn(2,N); % 控制變量的誤差2*N v = sqrt(R)*randn(1,N); % 測量誤差1*N ye_list = zeros(size(u)); % 估計值 yv_list = zeros(size(u)); % 測量值 y_list = zeros(size(u)); % 實際值 cov_list = zeros(size(u)); % 測量方差 for i = 1:numel(u)xreal = A*x0 + B*u(i); % 真實的狀態變量yreal = C*x0; % 真實的測量x1 = xreal + w(:,i); % 含噪聲的狀態變量yv = yreal + v(i); % 含噪聲的測量xfe = A*xh0 + B*u(i); % 先驗的狀態變量Pfe = A*P*A'+ Q; % 先驗的狀態變量協方差K = Pfe*C'/(C*Pfe*C'+R); % 卡爾曼最優增益xh1 = xfe + K*(yv-C*xfe); % 當前的狀態估計ye = C*xh1;P = (I-K*C)*Pfe;x0 = x1;xh0 = xh1;y_list(i) = yreal;yv_list(i) = yv;ye_list(i) = ye;cov_list(i) = C*P*C'; end ax = (1:N).*dt; figure(1); subplot(2,2,1) plot(ax,y_list,ax,yv_list,ax,ye_list) legend('實際','測量','估計','Location','best') title('汽車的位移') ylabel('位移/m') xlabel('時間/s') subplot(2,2,2) plot(ax,yv_list-y_list,ax,ye_list-y_list) legend('測量','估計','Location','best') title('汽車的位移誤差') ylabel('位移/m') xlabel('時間/s') subplot(2,2,[3,4]) plot(ax,cov_list) legend('測量方差','Location','best') title('測量方差') ylabel('方差/m^2') xlabel('時間/s')

本文設定的采樣間隔$dt=0.1，x_0=[0.2,0{]}^T,{\hat{x}}_0=[0,0{]}^T$，注意由于$w=B{w}^{\prime }$，而B的第一行為0，故$Q$的對角線第一元素必定為0.仿真的效果圖如下：

實際值即狀態方程的輸出$y=Cx$，測量值即含噪聲的輸出$y_v=Cx+v$，估計值為卡爾曼濾波器對測量的最優估計值。
\qquad希望本文對您有所幫助，謝謝閱讀！如有異議，歡迎評論區指正！

另外對本話題感興趣的讀者可以繼續閱讀有關適用于非線性系統的擴展卡爾曼濾波算法的介紹。

創作挑戰賽新人創作獎勵來咯，堅持創作打卡瓜分現金大獎

總結

以上是生活随笔為你收集整理的【MATLAB】卡尔曼滤波器的原理及仿真（初学者专用）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。