卡爾曼濾波器學習筆記（二）

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

經典的卡爾曼濾波只適用於線性且滿足高斯分佈的系統，但實際工程中並不是這麼簡單，比如飛行器在水平運動時有可能伴隨着自身的自旋，此時的系統並不是線性的，這時就需要應用擴展卡爾曼濾波（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:N
  z = h(s) + r*randn;                     
  sV(:,k)= s;                             %實際狀態
  zV(:,k)  = z;                           %狀態測量值
  [x1,A]=jaccsd(f,x); %計算f的雅可比矩陣，其中x1對應黃金公式line2
  P=A*P*A'+Q;         %過程方差預測，對應line3
  [z1,H]=jaccsd(h,x1); %計算h的雅可比矩陣
  K=P*H'*inv(H*P*H'+R); %卡爾曼增益，對應line4
  x=x1+K*(z-z1);        %狀態EKF估計值，對應line5
  P=P-K*H*P;            %EKF方差，對應line6
  xV(:,k) = x;          %save
  s = f(s) + q*randn(3,1);  %update process 
end
for k=1:3
  FontSize=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:n
    x1=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.