扩展卡尔曼滤波器的原理及应用

http://blog.csdn.net/lizilpl/article/details/45289541

经典的卡尔曼滤波只适用于线性且满足高斯分布的系统，但实际工程中并不是这么简单，比如飞行器在水平运动时有可能伴随着自身的自旋，此时的系统并不是线性的，这时就需要应用扩展卡尔曼滤波（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.