迭代扩展卡尔曼滤波算法-IEKF

汽车安全 3年前 (2021) admin

2,250 0 0

1、EKF算法原理

EKF全称ExtendedKalman Filter，即扩展卡尔曼滤波器，一种高效率的递归滤波器（自回归滤波器）。基本的卡尔曼滤波器在线性系统中有着良好的性能，但在非线性系统中效果不理想，一般适用于线性系统。扩展卡尔曼滤波器通过对非线性系统进行线性化使得卡尔曼滤波器在非线性系统中也可以有良好的性能。

对于线性系统：

迭代扩展卡尔曼滤波算法-IEKF

式中，xt和xt-1是t时刻和t-1时刻的状态向量，ut是t时刻的控制向量，εt是系统的过程噪声，δt是系统的测量噪声，对于线性系统，卡尔曼滤波算法可以表述为：

迭代扩展卡尔曼滤波算法-IEKF

扩展卡尔曼滤波系统：

考虑更加一般非线性系统：

迭代扩展卡尔曼滤波算法-IEKF

上式中的相关变量定义与线性系统定义相同，g(.)，h(.)为非线性函数。其不满足卡尔曼滤波算法对系统必须是线性系统的假设，因而不能其使用卡尔曼滤波器。

为对非线性应用卡尔曼滤波算法，可以对该非线性系统进行局部线性化，将线性化点周围的系统状态看做是线性系统，这样对于非线性不太强的系统，可以认为线性化点周围的系统状态是满足卡尔曼滤波算法的条件的。

扩展卡尔曼算法所使用的线性化方法为泰勒展开。对式中的g(ut, xt–1)和h(xt)进行一阶泰勒展开：

线性化后扩展卡尔曼滤波算法可表述如下：

迭代扩展卡尔曼滤波算法-IEKF

2、IEKF算法原理

基本的EKF算法假设噪声形式为高斯噪声，但实际情况下噪声形式往往为有色噪声形式，并且EKF算法的线性化工作点往往不是输入状态真实的均值，而是一个估计的均值，这样的偏差会导致计算出的雅可比矩阵也不是最好的，且线性化过程丢弃了许多高阶项，这会导致EKF算法的性能下降，甚至导致滤波器发散。迭代扩展卡尔曼滤波算法IEKF能有效的改善这一问题，个人理解其基本的思想与残差逼近类似。IEKF会将上一步修正后的输出作为下一次迭代修正误差的输入值，通过不断迭代来修正误差减小误差，具体的IEKF算法的步骤可表达如下：

迭代扩展卡尔曼滤波算法-IEKF

3、代码实现

function [x,P] = KF_IEKF_update(x,P, z,R, hfun,hjac, N)%function [x,P] = KF_IEKF_update(x,P, z,R, hfun,hjac, N)%% 输入:%   x - x(k|k-1) - 预测状态%   P - P(k|k-1) - 预测协方差%   z - 观测值%   R - 观测误差%   hfun - 函数用于计算新息，给出了非线性观测模型: v = hfun(x,z);%   hjac - 计算观测模型雅可比矩阵的函数: H = hjac(x);%   N - IEKF更新的迭代次数%% 输出:%   x - x(k|k) - %   P - P(k|k) - %
xo= x; % 先验值Po= P;Poi= inv(P);Ri= inv(R);
for i=1:N % 迭代    H= feval(hjac,x);    P= calculate_P(Po,H,R);        v= feval(hfun,x,z);     x= calculate_x(v,x,P,xo,Poi,H,Ri);    end
H= feval(hjac,x); % 最后迭代P= calculate_P(Po,H,R);function P= calculate_P(P,H,R)HP= H*P;PHt= P*H';S= H*PHt + R;P= P - PHt * inv(S) * HP;P= make_symmetric(P);function x= calculate_x(v,x,P,xo,Poi,H,Ri)M1= P * H' * Ri; M2= P * Poi * (x-xo);x= x + M1*v - M2;function P= make_symmetric(P)P= (P+P')/2;