文章目錄

• 0.引言
• 1.場(chǎng)景預(yù)設(shè)
• 2.卡爾曼濾波器
• 3.仿真及效果

0.引言

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

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

另外提供友情鏈接如下：

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

Matlab關(guān)于傳感器融合知識(shí)的介紹

擴(kuò)展卡爾曼濾波算法（CSDN，適合入門(mén)）

擴(kuò)展卡爾曼濾波算法（知乎，適合入門(mén)）

擴(kuò)展卡爾曼濾波算法（知乎，適合進(jìn)階）

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

\qquad首先，KF是一種狀態(tài)觀測(cè)器。狀態(tài)觀測(cè)器是針對(duì)可觀系統(tǒng)，根據(jù)輸出$y$和輸入$u$對(duì)系統(tǒng)內(nèi)部狀態(tài)$u$進(jìn)行觀測(cè)的結(jié)構(gòu)單元。其構(gòu)圖如下（參考視頻鏈接）：

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

在輸入$u$和動(dòng)態(tài)系統(tǒng)Plant中間會(huì)引入過(guò)程噪聲$w$，而在輸出$y$（即測(cè)量）和實(shí)際測(cè)量$y_v$中間會(huì)引入傳感器噪聲$v$,而卡爾曼濾波器則是根據(jù)$u,y_v$求得測(cè)量的最優(yōu)估計(jì)$y_e$。

1.場(chǎng)景預(yù)設(shè)

\qquad 假設(shè)一輛汽車(chē)在做勻速直線運(yùn)動(dòng)（一維場(chǎng)景），駕駛員通過(guò)油門(mén)控制了汽車(chē)的加速度恒定。忽略汽車(chē)所受的阻力、質(zhì)量變化，并假設(shè)駕駛員操作會(huì)給汽車(chē)的牽引力造成一定的過(guò)程噪聲。選擇汽車(chē)的位移和速度為狀態(tài)變量$x=[p,v{]}^T$，則狀態(tài)變量導(dǎo)數(shù)為汽車(chē)的速度和加速度即$\dot{x}=[v,a{]}^T$，選取控制變量$u=a$，測(cè)量量為汽車(chē)的位移。則狀態(tài)方程如下：
$$?\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$，則狀態(tài)方程離散化后變?yōu)?#xff1a;
$$?\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]$$
由于系統(tǒng)具有一定的過(guò)程噪聲$w$和測(cè)量噪聲$v$。引入噪聲的離散系統(tǒng)狀態(tài)方程為：
$$\{\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}$$
為加以區(qū)別，使用$y_v$在本文中表示含噪聲的真實(shí)測(cè)量。定義隨機(jī)噪聲的方差：
$E((w?\overline{w}{)}^T(w?\overline{w}))=Q,E((v?\overline{v}{)}^T(v?\overline{v}))=R$
汽車(chē)的初始真實(shí)位移為0.2，真實(shí)速度為0即$x_0=[0.2,0{]}^T$.

2.卡爾曼濾波器

\qquad為方便初學(xué)者入門(mén)，本文不會(huì)從貝葉斯估計(jì)的角度證明KF的公式，但會(huì)將其應(yīng)用步驟以盡可能簡(jiǎn)單的手段列出。

需要設(shè)定一個(gè)初始的狀態(tài)估計(jì)值即${\hat{x}}_0$，需要認(rèn)為規(guī)定初始的先驗(yàn)估計(jì)的協(xié)方差矩陣$P_0=E((x?\hat{x}{)}^T(x?\hat{x}))$。${\hat{x}}_0$一般是憑經(jīng)驗(yàn)設(shè)定的，如果系統(tǒng)偏離平衡狀態(tài)不遠(yuǎn)設(shè)定$[0,0{]}^T$也無(wú)妨；$P_0$在教學(xué)視頻中并未給出設(shè)定方式，一般設(shè)定為$Q$即可。令$k=1$，仿真開(kāi)始。

獲取當(dāng)前測(cè)量量$y_v$，先求出$x,P$的先驗(yàn)估計(jì)，即$$\begin{gathered} {\hat{x}}_k^{?}=A{\hat{x}}_{k?1}+B{u}_k \\ {P}_k^{?}=A{P}_{k?1}{A}^T+Q \end{gathered}$$

根據(jù)$x,P$的先驗(yàn)估計(jì)，求出$x$的后驗(yàn)估計(jì)$\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}$$

根據(jù)$\hat{y}=C\hat{x}$求出測(cè)量的卡爾曼估計(jì)值$\hat{y}$.

$k=k+1$,轉(zhuǎn)步2

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

3.仿真及效果

在此附上Matlab的仿真代碼

% 模擬要求汽車(chē)在一維空間的加速和減速過(guò)程 % 控制變量u是汽車(chē)的加速度 % 狀態(tài)變量x=[p,v],x^hat=[v,a] % w為控制變量的隨機(jī)擾動(dòng)，v為測(cè)量的隨機(jī)擾動(dòng) % Q為w的方差，R為v的方差，假設(shè)w與v相互獨(dú)立 clear dt = 0.1; % 采樣間隔 N = 100; % 仿真數(shù) 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的估計(jì) u = ones(1,N); % 加速度恒定 w = sqrt(Q)*randn(2,N); % 控制變量的誤差2*N v = sqrt(R)*randn(1,N); % 測(cè)量誤差1*N ye_list = zeros(size(u)); % 估計(jì)值 yv_list = zeros(size(u)); % 測(cè)量值 y_list = zeros(size(u)); % 實(shí)際值 cov_list = zeros(size(u)); % 測(cè)量方差 for i = 1:numel(u)xreal = A*x0 + B*u(i); % 真實(shí)的狀態(tài)變量yreal = C*x0; % 真實(shí)的測(cè)量x1 = xreal + w(:,i); % 含噪聲的狀態(tài)變量yv = yreal + v(i); % 含噪聲的測(cè)量xfe = A*xh0 + B*u(i); % 先驗(yàn)的狀態(tài)變量Pfe = A*P*A'+ Q; % 先驗(yàn)的狀態(tài)變量協(xié)方差K = Pfe*C'/(C*Pfe*C'+R); % 卡爾曼最優(yōu)增益xh1 = xfe + K*(yv-C*xfe); % 當(dāng)前的狀態(tài)估計(jì)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('實(shí)際','測(cè)量','估計(jì)','Location','best') title('汽車(chē)的位移') ylabel('位移/m') xlabel('時(shí)間/s') subplot(2,2,2) plot(ax,yv_list-y_list,ax,ye_list-y_list) legend('測(cè)量','估計(jì)','Location','best') title('汽車(chē)的位移誤差') ylabel('位移/m') xlabel('時(shí)間/s') subplot(2,2,[3,4]) plot(ax,cov_list) legend('測(cè)量方差','Location','best') title('測(cè)量方差') ylabel('方差/m^2') xlabel('時(shí)間/s')

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

實(shí)際值即狀態(tài)方程的輸出$y=Cx$，測(cè)量值即含噪聲的輸出$y_v=Cx+v$，估計(jì)值為卡爾曼濾波器對(duì)測(cè)量的最優(yōu)估計(jì)值。
\qquad希望本文對(duì)您有所幫助，謝謝閱讀！如有異議，歡迎評(píng)論區(qū)指正！

另外對(duì)本話題感興趣的讀者可以繼續(xù)閱讀有關(guān)適用于非線性系統(tǒng)的擴(kuò)展卡爾曼濾波算法的介紹。

創(chuàng)作挑戰(zhàn)賽新人創(chuàng)作獎(jiǎng)勵(lì)來(lái)咯，堅(jiān)持創(chuàng)作打卡瓜分現(xiàn)金大獎(jiǎng)

總結(jié)

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

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。