文章目錄

• 0.引言及友情鏈接
• 1.場景預(yù)設(shè)
• 2.擴(kuò)展卡爾曼濾波器
• 3.仿真及效果

0.引言及友情鏈接

\qquad卡爾曼濾波器（Kalman Filter, KF）是傳感器融合（Sensor Fusion）的基礎(chǔ)，雖然知乎、CSDN、GitHub等平臺已有大量的學(xué)習(xí)資料，但還是建議大家在看完B站Matlab Tech Talk有關(guān)卡爾曼濾波器視頻后再進(jìn)入深入學(xué)習(xí)。友情鏈接提供如下：

• B站Matlab官方視頻
• 卡爾曼濾波器介紹（CSDN，初學(xué)者專用）
• 卡爾曼濾波器介紹（知乎，進(jìn)階篇）
• 擴(kuò)展卡爾曼濾波器原理（知乎，適合入門）
• 擴(kuò)展卡爾曼與無跡卡爾曼（知乎，進(jìn)階篇）
• 擴(kuò)展卡爾曼濾波器公式推導(dǎo)（Github.io）

\qquad在此感謝各位辛勤的知乎、CSDN作者及B站分享視頻的Up主。未學(xué)習(xí)卡爾曼濾波器的讀者可學(xué)習(xí)鏈接中的1和2，本文將介紹擴(kuò)展卡爾曼濾波器（Extended Kalman Filter, EKF）的原理并以一個有關(guān)汽車運(yùn)動的Matlab仿真說明其應(yīng)用。
\qquad看過我上一篇介紹KF的博客的讀者肯定知道KF設(shè)計的目的和結(jié)構(gòu)，即讓狀態(tài)估計的方差隨時間推移趨于0，然而由于KF針對的是隨機(jī)系統(tǒng)，這一點(diǎn)往往做不到，而只能使其收斂為一個常數(shù)。EKF和KF的結(jié)構(gòu)差不多，只不過EKF針對的是非線性系統(tǒng)的濾波器。不含隨機(jī)噪聲的非線性系統(tǒng)狀態(tài)方程如下：
Nonlinear System:
$$\{\begin{array}{l}x(k)=f(x(k?1),u(k?1))\\ y(k)=g(x(k),u(k))\end{array}$$
引入EKF后，其結(jié)構(gòu)框圖如下：

\qquad其中$\hat{x}$為估計狀態(tài)，$w$為過程噪聲（一般由控制變量$u$引入，但也可能由物理系統(tǒng)本身的不確定性引入），而$v$為測量噪聲。根據(jù)Matlab的幫助文檔介紹，噪聲也分為兩種——加入型噪聲（Additive Noise）和非加入型噪聲（Nonadditive Noise），其分類取決于噪聲是否在非線性函數(shù)內(nèi)部，二者的狀態(tài)方程形式如下：
Additive Noise:
$$\{\begin{array}{l}x(k)=f(x(k?1),u(k))+w(k)\\ y(k)=g(x(k),u(k))+v(k)\end{array}$$
Nonadditive Noise
$$\{\begin{array}{l}x(k)=f(x(k?1),u(k),w(k))\\ y(k)=g(x(k),u(k),v(k))\end{array}$$
從上述表達(dá)式也可以看出加入型噪聲是非加入型的特例。

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

\qquad為了應(yīng)用EKF，需要構(gòu)造一個非線性系統(tǒng)，與前一篇講述KF的博文保持連續(xù)性。這次使用的仍然所示一維的汽車運(yùn)動模型，狀態(tài)變量仍然選擇汽車的位移和速度$(x=[p,v{]}^T)$，但這次控制變量為$u(k)$為汽車的功率/汽車的質(zhì)量，重新構(gòu)造狀態(tài)方程如下：
$$\{\begin{array}{l}\dot{x}=f(x,u)=A_0^{?}x+B_0^{?T}u/(B_0^{?}x)\\ y=g(x)=\frac{1}{2}(C_0^{?}x{)}^T(C_0^{?}x)\end{array}$$
其中$$A_0^{?}=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right],B_0^{?}=C_0^{?}=\left[\begin{array}{cc}0& 1\end{array}\right]$$
為了與控制變量$u$保持一致性，此處的測量$y$為單位質(zhì)量產(chǎn)生的動能。為了不失一般性，添加非加入型噪聲如下（同樣以$y_v$表示測量值，$y$表示實(shí)際值，$\hat{y}$表示估計值）：
$$\{\begin{array}{l}\dot{x}=f(x,u)=A_0^{?}x+B_0^{?T}(u+w)/(B_0^{?}x)\\ y_v=g(x)=\frac{1}{2}(C_0^{?}x{)}^T(C_0^{?}x)+v\end{array}$$
設(shè)定采樣時間$dt$，狀態(tài)方程化為離散形式：
$$\{\begin{array}{l}x(n)=f(x(n?1),u(n?1))=A_0+B_0^T(u(n?1)+w)/(B_{0}x(n?1))\\ y_v(n)=g(x(n))=\frac{1}{2}(C_{0}x(n){)}^T(C_{0}x(n))+v\end{array}$$
其中$$A_0=\left[\begin{array}{cc}1& dt\\ 0& 1\end{array}\right],B_0=\left[\begin{array}{cc}0& 1\end{array}\right]$$
與上一篇博文不同，設(shè)$$E((B_{0}w?\overline{B_{0}w}{)}^T(B_{0}w?\overline{B_{0}w}))=Q,E((v?\overline{v}{)}^T(v?\overline{v}))=R$$

2.擴(kuò)展卡爾曼濾波器

\qquad與上一篇博文一樣，本文不會從概率論或者最優(yōu)化理論的角度對EKF的公式加以深入推導(dǎo)，但會詳細(xì)列出EKF最優(yōu)估計的算法步驟。步驟會與Matlab的幫助文檔的計算順序略有出入，但經(jīng)過實(shí)驗(yàn)比較之后結(jié)果幾乎沒有差異。

設(shè)定初始狀態(tài)變量的估計值${\hat{x}}_0$，并計算以下導(dǎo)數(shù)及P的初始值：$$\begin{gathered} A_0=\frac{?f}{?x}{|}_{{\hat{x}}_0,u_0} \\ {G}_0=\frac{?f}{?w}{|}_{{\hat{x}}_0,u_0} \\ {C}_0=\frac{?g}{?x}{|}_{{\hat{x}}_0} \\ {S}_0=\frac{?g}{?v}{|}_{{\hat{x}}_0} \\ {P}_0=G_{0}Q{G}_0^T \end{gathered}$$令$k=1$

獲取當(dāng)前測量量$y_v$，計算狀態(tài)變量的先驗(yàn)估計${\hat{x}}_k^{?}=f({\hat{x}}_{k?1},u_{k?1})$

計算以下導(dǎo)數(shù)：$$\begin{gathered} A_k=\frac{?f}{?x}{|}_{{\hat{x}}_k^{?},u_k} \\ {G}_k=\frac{?f}{?w}{|}_{{\hat{x}}_k^{?},u_k} \\ {C}_k=\frac{?g}{?x}{|}_{{\hat{x}}_k^{?}} \\ {S}_k=\frac{?g}{?v}{|}_{{\hat{x}}_k^{?}} \end{gathered}$$并順帶計算$P_k^{?}=A_{k}P{A}_k^T+G_{k}Q{G}_k^T$

計算EKF的最優(yōu)增益$K_k=P_k^{?}{C}_k^T(C_k{P}_k^{?}{C}_k^T+S_{k}R{S}_k^T{)}^{?1}$

更新$P_k=(I?K_k{C}_k)P_k^{?}$并計算狀態(tài)變量的后驗(yàn)估計${\hat{x}}_k={\hat{x}}_k^{?}+K_k(y_v?g(x_k^{?}))$

計算測量量的估計值${\hat{y}}_k=g({\hat{x}}_k)$，令$k=k+1$，轉(zhuǎn)步2

3.仿真及效果

仿真的Matlab代碼如下：

% 模擬要求汽車在一維空間的加速和減速過程 % 控制變量u是汽車的加速度 % 狀態(tài)變量x=[p,v],x^hat=[v,a] % w為控制變量的隨機(jī)擾動，v為測量的隨機(jī)擾動 % Q為w的方差，R為v的方差，假設(shè)w與v相互獨(dú)立 clear dt = 0.1; % 采樣間隔 m = 1e3; % 汽車自重 N = 100; % 仿真數(shù) Q = 2e-4; % 過程噪聲的協(xié)方差矩陣 R = 0.01; % 測量噪聲的方差 x0 = [0;0.5]; % 初始位置和速度 xh0 = [1;0.4]; % x0的估計 A0 = [1,dt;0,1]; B0 = [0,1]; f = @(x,u)(A0*x+B0'*dt*u./(B0*x)); % 系統(tǒng)方程的非線性函數(shù) C0 = sqrt([0,10]); g = @(x,v)(1/2*(C0*x)'*(C0*x)+v); % 輸出方程的非線性函數(shù) A = @(x,u)(A0+[0,0;0,-dt*u/x(2)^2]); % pf/px 2*2 G = @(x)([0;-dt/x(2)]); % pf/pw 2*1 C = @(x)(C0.*x'); % pg/px 1*2 S = 1; % pg/pv 2*1 P = G(xh0)*Q*G(xh0)'; % 2*2 I = eye(2); u = 0.01*ones(1,N); % 功率恒定 1*1 w = sqrt(Q)*randn(1,N); % 控制變量的誤差1*N v = sqrt(R)*randn(1,N); % 測量誤差1*N ye_list = zeros(size(u)); % 估計值 yv_list = zeros(size(u)); % 測量值 y_list = zeros(size(u)); % 實(shí)際值 cov_list = zeros(size(u)); % 測量方差 for i = 1:numel(u)xreal = f(x0,u(i)); % 真實(shí)的狀態(tài)變量yreal = g(x0,0); % 真實(shí)的測量x1 = f(x0,u(i)+w(i)); % 含噪聲的狀態(tài)變量 2*1yv = g(x0,v(i)); % 含噪聲的測量 1*1xfe = f(xh0,u(i));Pfe = A(xfe,u(i))*P*A(xfe,u(i))'+G(xfe)*Q*G(xfe)';K = Pfe*C(xfe)'/(C(xfe)*Pfe*C(xfe)'+S*R*S'); % 卡爾曼最優(yōu)增益 2*1P = (I - K*C(xfe))*Pfe; % 當(dāng)前狀態(tài)先驗(yàn)估計協(xié)方差xh1 = xfe+K*(yv-g(xfe,0)); % 狀態(tài)預(yù)測ye = g(xh1,0);x0 = x1;xh0 = xh1;y_list(i) = yreal;yv_list(i) = yv;ye_list(i) = ye;cov_list(i) = C(xh1)*P*C(xh1)'; end ax = (1:N).*dt; figure(1); subplot(2,2,1) plot(ax,y_list,ax,yv_list,ax,ye_list) legend('實(shí)際','測量','估計','Location','best') title('汽車的動能') ylabel('動能/J') xlabel('時間/s') subplot(2,2,2) plot(ax,yv_list-y_list,ax,ye_list-y_list) legend('測量','估計','Location','best') title('汽車的動能誤差') ylabel('動能/J') xlabel('時間/s') subplot(2,2,[3,4]) plot(ax,cov_list) legend('測量方差','Location','best') title('測量方差') ylabel('方差/m^2') xlabel('時間/s')

\qquad這里設(shè)定的汽車的功率/質(zhì)量為恒定的0.01，汽車初速度為1m/s，為恒功率加速過程，根據(jù)能量守恒定律，其測量量（單位質(zhì)量的動能）應(yīng)成近似線性增長。相關(guān)效果圖如下：

\qquad通過效果圖可以看出，雖然初始狀態(tài)時估計值${\hat{x}}_0$與真實(shí)值$x_0$相差較大造成EKF的誤差較測量值較大，但是經(jīng)過一段時間推移后，EKF的測量誤差逐漸減小并較測量值有了顯著提升。EKF算法對于非線性系統(tǒng)是近似收斂的，從測量方差的走勢也可以看出。需要指出的是這里的測量方差是單位質(zhì)量的動能，真實(shí)的動能需要乘上汽車的質(zhì)量。
\qquad希望本文對您有幫助，謝謝閱讀。若有異議，歡迎評論區(qū)指正！

總結(jié)

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

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