扩展卡尔曼滤波EKF，与LKF区别。IMU and GPS Fusion for Inertial Navigation，MATLAB实例学习。

MATLAB：Sensor Fusion and Tracking Toolbox

实例一：IMU and GPS Fusion for Inertial Navigation

 一、IMUandGPSFusionExample.m

1、IMU的加速度计、陀螺仪的采样频率很高。Conversely，磁力计与GPS的采样频率较低。

2、数据：

        IMU：

                Orientation ，磁力计 ，用四元数表示

                AngularVelocity，陀螺仪

                Acceleration，加速度计

        GPS：

                Position

                Velocity

3、代码目标

        总体目标：将来自IMU的数据、与来自GPS的数据，通过EKF算法融合新的Position、Orientation。

        具体步骤：

        （1）读入真实的数据。

       （2）利用imu、gps模型（噪声参数自己设置），仿真得到IMU、GPS传感器的测量数据。

[accel, gyro, mag] = imu(trajAcc(fcnt,:), trajAngVel(fcnt, :), trajOrient(fcnt));

[lla, gpsvel] = gps( trajPos(fcnt,:), trajVel(fcnt,:) );

      （3）通过IMU的 acc 、gyro数据，用predict函数去预测 Position and Orientation。

predict(fusionfilt, accel, gyro);

      （4）通过GPS的position数据，用fusegps函数去做Position的更新。通过IMU的magn数据，用fusemag函数去做Orientation的更新。

fusegps(fusionfilt, lla, Rpos, gpsvel, Rvel);
fusemag(fusionfilt, mag, Rmag);

   

二、核心代码 MARGGPSFuserBase.m文件

        1、 AccelerometerNoise = [1e-4 1e-4 1e-4]；加速度计噪声的协方差

        2、AccelerometerBiasNoise = [1e-4 1e-4 1e-4]；是加速度计bias协方差。

                这里IMU的噪声模型参考这篇文章：IMU误差模型与校准 - 修禅 - 博客园

                ①、noise是高斯噪声

                ②、bias是随机游走噪声、bias指零偏。

        3、% Multiplicative Process Noises

                乘法噪声

        4、% Additive Process Noises

                加法噪声

        5、预测过程

% Extended Kalman Filter predict algorithm

xk：k时刻的状态
dang：角度变化
dvel：速度变化
dt：delat T

Xnext：K+1时刻的状态
P：协方差
           
xnext = obj.stateTransFcn(xk, dang, dvel, dt);  %相当于A
dfdx = obj.stateTransJacobianFcn(xk, dang, dvel, dt);   %计算一阶雅克比           
dwdx = obj.processNoiseJacobianFcn(xk, multNoise);  %计算乘法噪声的一阶雅克比
Pnext = dfdx * P * (dfdx.') + dwdx  + addProcNoise; % 代替 APA+Q
xk = xnext;
P = Pnext;

        6、更新过程

% Basic EKF correct

xk = obj.State;
h = measFcn(obj, xk);  % measFcn：观测方程 ，h是xk的观测值，相当于H*xk
innov = z - h;  % z：观测值，innov是观测误差 xest = xk + K*innov
dhdx = measJacobianFcn(obj, xk);
P = obj.StateCovariance;
[xest, P] = correctEqn(obj, xk, P, h, dhdx, z, measNoise);
obj.StateCovariance = P;
obj.State = xest;

       function [x, P] = correctEqn(obj, x, P, h, H, z, R)
            S = H*P*(H.') + R;
            W = P*(H.') / S;
            
            x = x + W*(z-h);
            P = P - W*H*P;
        end 

        7、EKF扩展卡尔曼滤波与LKF线性卡尔曼滤波区别

                ①线性卡尔曼滤波流程

                    ②、EKF体现：

                        1）在预测过程中：

                                状态估计：用obj.stateTransFcn 代替 A.*

                                计算P：用 Pnext = dfdx * P * (dfdx.') + dwdx  + addProcNoise 代替 APA+Q

                        3）在更新过程：

                                用 dhdx 代替 H。

        8、补充

                卡尔曼滤波主要是如何确定P的初值，只要不为0，对于初值不敏感（没有试过）。