當前位置：首頁 > 编程资源 > 编程问答 >内容正文

编程问答

卡尔曼滤波器学习笔记（二）

發布時間：2024/4/18 编程问答 41 豆豆

生活随笔收集整理的這篇文章主要介紹了卡尔曼滤波器学习笔记（二）小編覺得挺不錯的,現在分享給大家,幫大家做個參考.

擴展卡爾曼濾波器的原理及應用

經典的卡爾曼濾波只適用于線性且滿足高斯分布的系統，但實際工程中并不是這么簡單，比如飛行器在水平運動時有可能伴隨著自身的自旋，此時的系統并不是線性的，這時就需要應用擴展卡爾曼濾波（EKF）來解決這種情況

應用前提
EKF算法詳細介紹
應用舉例
下一步

1.應用前提

與kalman Filter只能應用于線性系統不同,Extended Kalman Filter 可以用于非線性系統中。：

當前狀態的概率分布是關于上一狀態和將要執行的控制量的二元函數，再疊加一個高斯噪聲，測量值同樣是關于當前狀態的函數疊加高斯噪聲。具體表達式如下：

和可以是非線性的函數。
為了用經典卡爾曼濾波器的思想來解決非線性系統中的狀態估計問題，首先要做的就是把?和用泰勒級數展開，將其線性化，只取一次項為一階EKF濾波。具體如下：

在上一狀態估計的最優值處取一階導數，在當前時刻預測值處取一階導數，得到G和H分別相當于Kalman Filter中的A和C。

2.EKF算法詳細介紹

Extended Kalman Filter五條黃金公式?：

將其線性化之后，EFK與FK就非常類似了，所以我就不再贅述，可參考上一篇blog：卡爾曼濾波器的原理及應用?http://blog.csdn.net/lizilpl/article/details/45268471?
里面對這五條公式有較為詳細的介紹。

3.應用實例

下面這個小程序模擬了三維狀態非線性系統的EKF算法。

% author : Perry.Li @USTC % function: simulating the process of EKF % date: 04/28/2015 % N = 50; %計算連續N個時刻 n=3; %狀態維度 q=0.1; %過程標準差 r=0.2; %測量標準差 Q=q^2*eye(n); %過程方差 R=r^2; %測量值的方差 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); P = eye(n); xV = zeros(n,N); sV = zeros(n,N); zV = zeros(n,N); for k=1:Nz = h(s) + r*randn; sV(:,k)= s; %實際狀態zV(:,k) = z; %狀態測量值[x1,A]=jaccsd(f,x); %計算f的雅可比矩陣，其中x1對應黃金公式line2P=A*P*A'+Q; %過程方差預測，對應line3[z1,H]=jaccsd(h,x1); %計算h的雅可比矩陣K=P*H'*inv(H*P*H'+R); %卡爾曼增益，對應line4x=x1+K*(z-z1); %狀態EKF估計值，對應line5P=P-K*H*P; %EKF方差，對應line6xV(:,k) = x; %saves = f(s) + q*randn(3,1); %update process end for k=1:3FontSize=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('時間(分鐘)');t=['狀態 ',num2str(k)] ;yl=ylabel(t);set(xl,'fontsize',FontSize);set(yl,'fontsize',FontSize);hold off;set(gca,'FontSize',FontSize); end

用到的Function為計算 jacobi matrix，如下：

function [z,A]=jaccsd(fun,x) % JACCSD Jacobian through complex step differentiation % [z J] = jaccsd(f,x) % z = f(x) % J = f'(x) % z=fun(x); n=numel(x); m=numel(z); A=zeros(m,n); h=n*eps; for k=1:nx1=x;x1(k)=x1(k)+h*i;A(:,k)=imag(fun(x1))/h; end

運行結果如下：?
?
?

三個圖為三維狀態的真實值，測量值，EKF估計值對比圖，效果拔群！

4.下一步

如上討論，目前兩個算法都停留在模擬階段。接下來準備把EKF應用到自己的飛控板上，移植好之后放代碼，看效果！?
BTW, 附上本篇代碼下載鏈接，http://download.csdn.net/detail/lizilpl/8643227.

總結

以上是生活随笔為你收集整理的卡尔曼滤波器学习笔记（二）的全部內容，希望文章能夠幫你解決所遇到的問題。

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

上一篇：卡尔曼滤波器学习笔记（一）
下一篇： Pixhawk飞控源码目录结构及编译流程