IMU和GPS数据融合的扩展卡尔曼滤波器实现

1. 简介
   本文介绍一个基于扩展卡尔曼滤波器(EKF)的IMU和GPS数据融合算法实现。该算法可以将IMU的高频惯性数据和GPS的低频但精确的位置数据进行融合,得到更加准确的位置估计。

2. 系统状态向量
   系统状态向量包含16个量:

姿态四元数(4个)
陀螺仪偏置(3个)
位置(3个)
速度(3个)
加速度计偏置(3个)
3. 关键步骤
3.1 预测步骤
cpp
void predict(const MVec3& accelData, const MVec3& gyroData, double dt)
使用IMU数据进行状态预测:

通过状态转移方程更新状态向量
计算状态转移雅可比矩阵F
计算噪声雅可比矩阵G
更新状态协方差矩阵
3.2 更新步骤
cpp
void fusegps(MVec3 lla, double dt)
使用GPS数据进行修正:

计算测量预测值h
计算测量雅可比矩阵H
计算卡尔曼增益
更新状态向量和协方差矩阵
4. 坐标转换
使用lla2enu函数将GPS的经纬度坐标转换为ENU(东北天)坐标系。

5. 使用方法
   cpp
   // 初始化滤波器
   INSEKFfliter ekf(initialAtt, initialPos, initialVel);

// IMU预测
ekf.predict(accelData, gyroData, dt);

// GPS更新
ekf.fusegps(lla, dt);

// 获取位置
MVec3 pos = ekf.getpos();
6. 总结
该实现提供了一个完整的IMU/GPS融合框架,可以:

处理高频IMU数据
融合低频GPS数据
估计姿态、位置、速度等状态量
处理传感器偏置
主要应用于无人机、机器人等需要精确定位的场景。

// INSEKFfliter类 - IMU和GPS数据融合的扩展卡尔曼滤波器
class INSEKFfliter {
public:
    // 构造函数,初始化姿态、位置和速度
    INSEKFfliter(Eigen::Matrix<double, 4,1> initialAtt_, MVec3 initialPos_, MVec3 initialVel_);
    
    // IMU预测步骤
    void predict(const MVec3& accelData, const MVec3& gyroData, double dt);
    
    // GPS数据融合更新步骤 
    void fusegps(MVec3 lla, double dt);
    
    // 获取当前位置
    MVec3 getpos();

private:
    // IMU状态转移方程
    void IMUstateTranTcn(MVec3 accelMeas, MVec3 gyroMeas, double dt);
    
    // IMU状态转移雅可比矩阵
    Eigen::Matrix<double, 16,16> IMUstateTransitionJacobianFcn(MVec3 accelMeas, MVec3 gyroMeas, double dt);
    
    // IMU噪声雅可比矩阵
    Eigen::Matrix<double, 16,12> IMUnoiseJacobianFcn(Eigen::Matrix<double, 16,1>& x, double dt);
    
    // IMU噪声协方差矩阵
    Eigen::Matrix<double, 12,12> IMUnoiseCovariance();
    
    // 预测步骤协方差更新
    void predictCovEqnFcn(Eigen::Matrix<double, 16,16>& F,Eigen::Matrix<double, 12,12>& U, Eigen::Matrix<double, 16,12>& G);
    
    // GPS位置测量方程
    Eigen::Matrix<double, 3,1> GPSPositionmeasurementFcn(Eigen::Matrix<double, 16,1>& x);
    
    // GPS测量雅可比矩阵
    Eigen::Matrix<double, 3,16> GPSPositionmeasurementJacobianFcn(Eigen::Matrix<double, 16,1>& x);
    
    // 经纬度转ENU坐标
    MVec3 lla2enu(double lon, double lat);
    
    // 更新步骤
    void correctEqnFcn(Eigen::Matrix<double, 3,1>& h, Eigen::Matrix<double, 3,16>& H, const MVec3& pos, Eigen::Matrix<double, 3,3>& R);

帮我分析一些这部分代码是干什么用的，尝试帮我进行注释，然后最好能生产一篇csdn博客   //
// Created by Lenovo on 2021/8/13.
//

#ifndef IMU_GPS_EKF_CPP_INSEKFFLITER_H
#define IMU_GPS_EKF_CPP_INSEKFFLITER_H
#include <Eigen/Dense>
#include <vector>
typedef Eigen::Matrix<double, 3,1> MVec3;
class INSEKFfliter {
public:
    INSEKFfliter(Eigen::Matrix<double, 4,1> initialAtt_, MVec3 initialPos_, MVec3 initialVel_);
    void predict(const MVec3& accelData, const MVec3& gyroData, double dt);
    void fusegps(MVec3 lla, double dt);
    MVec3 getpos();
private:
    void IMUstateTranTcn(MVec3 accelMeas, MVec3 gyroMeas, double dt);
    Eigen::Matrix<double, 16,16> IMUstateTransitionJacobianFcn(MVec3 accelMeas, MVec3 gyroMeas, double dt);
    Eigen::Matrix<double, 16,12> IMUnoiseJacobianFcn(Eigen::Matrix<double, 16,1>& x, double dt);
    Eigen::Matrix<double, 12,12> IMUnoiseCovariance();
    void predictCovEqnFcn(Eigen::Matrix<double, 16,16>& F,Eigen::Matrix<double, 12,12>& U, Eigen::Matrix<double, 16,12>& G);

    Eigen::Matrix<double, 3,1> GPSPositionmeasurementFcn(Eigen::Matrix<double, 16,1>& x);
    Eigen::Matrix<double, 3,16> GPSPositionmeasurementJacobianFcn(Eigen::Matrix<double, 16,1>& x);
    MVec3 lla2enu(double lon, double lat);
    void correctEqnFcn(Eigen::Matrix<double, 3,1>& h, Eigen::Matrix<double, 3,16>& H, const MVec3& pos, Eigen::Matrix<double, 3,3>& R);
private:
    Eigen::Matrix<double, 4,1> initialAtt;
    MVec3 initialPos;
    MVec3 initialVel;
    Eigen::Matrix<double, 16,1> State;
    Eigen::Matrix<double, 16,16> StateCovariance;
    double accelBiasDecayFactor = 0.9999;
    double gyroBiasDecayFactor  = 0.999;
    double Rpos = 1;
    double GyroscopeNoise = 4e-6;
    double GyroscopeBiasNoise = 4e-14;
    double AccelerometerNoise = 4.8e-2;
    double AccelerometerBiasNoise = 4e-14;
};


#endif //IMU_GPS_EKF_CPP_INSEKFFLITER_H  #include "INSEKFfliter.h"
#include <iostream>
#include <fstream>

Eigen::Matrix<double, 4,1> initialAtt;/* NOLINT */
Eigen::Matrix<double, 3,1> initialPos;/* NOLINT */
Eigen::Matrix<double, 3,1> initialVel;/* NOLINT */
std::vector<MVec3> accelDatas;
std::vector<MVec3> gyroDatas;
std::vector<MVec3> llas;
std::vector<MVec3> truePositions;
std::vector<MVec3> estposs;
enum ReadState
{
    IDLE,
    READ_initialAtt,
    READ_initialPos,
    READ_initialVel,
    READ_accelDatas,
    READ_gyroDatas,
    READ_llas,
    READ_truePositions
};
void ReadDataFromTxt(const std::string& filename)
{
    std::ifstream textfile(filename);
    std::string line;
    ReadState state = IDLE;
    MVec3 tmpData;
    while (std::getline(textfile, line)) {
        int pos;
        pos = line.find("initialAtt");
        if(pos != std::string::npos) {
            state = READ_initialAtt;
            std::getline(textfile, line);
        }

        pos = line.find("initialPos");
        if(pos != std::string::npos) {
            state = READ_initialPos;
            std::getline(textfile, line);
        }

        pos = line.find("initialVel");
        if(pos != std::string::npos) {
            state = READ_initialVel;
            std::getline(textfile, line);
        }

        pos = line.find("accelDatas");
        if(pos != std::string::npos) {
            state = READ_accelDatas;
            std::getline(textfile, line);
        }

        pos = line.find("gyroDatas");
        if(pos != std::string::npos) {
            state = READ_gyroDatas;
            std::getline(textfile, line);
        }

        pos = line.find("llas");
        if(pos != std::string::npos) {
            state = READ_llas;
            std::getline(textfile, line);
        }

        pos = line.find("truePositions");
        if(pos != std::string::npos) {
            state = READ_truePositions;
            std::getline(textfile, line);
        }
        std::stringstream linstream(line);
        switch(state)
        {
            case READ_initialAtt:
                linstream>>initialAtt(0);
                linstream>>initialAtt(1);
                linstream>>initialAtt(2);
                linstream>>initialAtt(3);
                break;
            case READ_initialPos:
                linstream>>initialPos(0);
                linstream>>initialPos(1);
                linstream>>initialPos(2);
                break;
            case READ_initialVel:
                linstream>>initialVel(0);
                linstream>>initialVel(1);
                linstream>>initialVel(2);
                break;
            case READ_accelDatas:
                linstream>>tmpData(0);
                linstream>>tmpData(1);
                linstream>>tmpData(2);
                accelDatas.emplace_back(tmpData);
                break;
            case READ_gyroDatas:
                linstream>>tmpData(0);
                linstream>>tmpData(1);
                linstream>>tmpData(2);
                gyroDatas.emplace_back(tmpData);
                break;
            case READ_llas:
                linstream>>tmpData(0);
                linstream>>tmpData(1);
                linstream>>tmpData(2);
                llas.emplace_back(tmpData);
                break;
            case READ_truePositions:
                //std::cout<<linstream.str()<<std::endl;
                linstream>>tmpData(0);
                linstream>>tmpData(1);
                linstream>>tmpData(2);
                //std::cout<<tmpData<<std::endl;
                truePositions.emplace_back(tmpData);
                break;
            case IDLE:
                break;
        }
    }
}

int main() {
    std::string filename = ".\\trajData1.txt";
    std::cout << "IMU GPS EKF" << std::endl;
    ReadDataFromTxt(filename);
    std::cout<<"TruePositions size is "<<truePositions.size()<<std::endl;
    int cntIMU = 0;
    int cntGPS = 0;
    INSEKFfliter insekFfliter(initialAtt, initialPos, initialVel);
    double dt = 0.01;
    for(int i = 0; i < accelDatas.size()/10; ++i)
    {
        for(int j = 0; j < 10; ++j)
        {
            MVec3 accelData = accelDatas[cntIMU];
            MVec3 gyroData  = gyroDatas[cntIMU];
            cntIMU++;
            insekFfliter.predict(accelData, gyroData, dt);
            estposs.emplace_back(insekFfliter.getpos());
        }
        MVec3 lla = llas[cntGPS];
        cntGPS++;
        insekFfliter.fusegps(lla, dt);
        estposs.emplace_back(insekFfliter.getpos());
    }
    for(int i = 0; i < std::min(truePositions.size(), estposs.size()); ++i)
    {
        auto truePosition = truePositions[i];
        auto estpos       = estposs[i];
        std::cout<< truePosition[0]<<","<<truePosition[1]<<","<<estpos[0]<<","<<estpos[1]<<std::endl;
    }
    return 0;
}
   //
// Created by Lenovo on 2021/8/13.
//

#include "INSEKFfliter.h"

INSEKFfliter::INSEKFfliter(Eigen::Matrix<double, 4,1> initialAtt_, MVec3 initialPos_, MVec3 initialVel_):
initialAtt(std::move(initialAtt_)),initialPos(std::move(initialPos_)),initialVel(std::move(initialVel_))
{
    State(0) = initialAtt(0);
    State(1) = initialAtt(1);
    State(2) = initialAtt(2);
    State(3) = initialAtt(3);
    //Gyroscope.ConstantBias
    State(4) = 0;
    State(5) = 0;
    State(6) = 0;

    State(7) = initialPos(0);
    State(8) = initialPos(1);
    State(9) = initialPos(2);

    State(10) = initialVel(0);
    State(11) = initialVel(1);
    State(12) = initialVel(2);

    //Accelerometer.ConstantBias
    State(13) = 0;
    State(14) = 0;
    State(15) = 0;
    StateCovariance = Eigen::Matrix<double, 16,16>::Ones(16,16)*1e-9;
}
void INSEKFfliter::predict(const MVec3& accelData, const MVec3& gyroData, double dt)
{
    IMUstateTranTcn(accelData, gyroData, dt);
    Eigen::Matrix<double, 16,16> F = IMUstateTransitionJacobianFcn(accelData, gyroData, dt);
    Eigen::Matrix<double, 16,12> G = IMUnoiseJacobianFcn(State, dt);
    Eigen::Matrix<double, 12,12> U = IMUnoiseCovariance();
    predictCovEqnFcn(F, U, G);
}
void INSEKFfliter::fusegps(MVec3 lla, double dt)
{
    auto h = GPSPositionmeasurementFcn(State);
    auto H = GPSPositionmeasurementJacobianFcn(State);
    auto pos = lla2enu(lla(1), lla(0)); 
    Eigen::Matrix<double, 3,3> R = Eigen::Matrix<double, 3,3>::Identity(3,3)*Rpos;
    correctEqnFcn(h, H, pos, R);
}
MVec3 INSEKFfliter::getpos()
{
    MVec3 ret;
    ret(0) = State(7);
    ret(1) = State(8);
    ret(2) = State(9);
    return ret;
}

//private
void INSEKFfliter::IMUstateTranTcn(MVec3 accelMeas, MVec3 gyroMeas, double dt)
{
    double q0  = State(1-1);
    double q1  = State(2-1);
    double q2  = State(3-1);
    double q3  = State(4-1);
    double gbX = State(5-1);
    double gbY = State(6-1);
    double gbZ = State(7-1);
    double pn  = State(8-1);
    double pe  = State(9-1);
    double pd  = State(10-1);
    double vn  = State(11-1);
    double ve  = State(12-1);
    double vd  = State(13-1);
    double abX = State(14-1);
    double abY = State(15-1);
    double abZ = State(16-1);

    double amX = accelMeas(1-1);
    double amY = accelMeas(2-1);
    double amZ = accelMeas(3-1);
    double gmX = gyroMeas(1-1);
    double gmY = gyroMeas(2-1);
    double gmZ = gyroMeas(3-1);

    double lambdaAccel = 1-accelBiasDecayFactor;
    double lambdaGyro = 1-gyroBiasDecayFactor;

    double gravX = 0.0;
    double gravY = 0.0;
    double gravZ = 9.81;
    State(0) = q0 + dt*q1*(gbX/2 - gmX/2) + dt*q2*(gbY/2 - gmY/2) + dt*q3*(gbZ/2 - gmZ/2);
    State(1) = q1 - dt*q0*(gbX/2 - gmX/2) + dt*q3*(gbY/2 - gmY/2) - dt*q2*(gbZ/2 - gmZ/2);
    State(2) = q2 - dt*q3*(gbX/2 - gmX/2) - dt*q0*(gbY/2 - gmY/2) + dt*q1*(gbZ/2 - gmZ/2);
    State(3) = q3 + dt*q2*(gbX/2 - gmX/2) - dt*q1*(gbY/2 - gmY/2) - dt*q0*(gbZ/2 - gmZ/2);
    State(4) = -gbX*(dt*lambdaGyro - 1);
    State(5) = -gbY*(dt*lambdaGyro - 1);
    State(6) = -gbZ*(dt*lambdaGyro - 1);
    State(7) = pn + dt*vn;
    State(8) = pe + dt*ve;
    State(9) = pd + dt*vd;
    State(10) = vn + dt*(q0*(q0*(abX - amX) - q3*(abY - amY) + q2*(abZ - amZ)) - gravX + q2*(q1*(abY - amY) - q2*(abX - amX) + q0*(abZ - amZ)) + q1*(q1*(abX - amX) + q2*(abY - amY) + q3*(abZ - amZ)) - q3*(q3*(abX - amX) + q0*(abY - amY) - q1*(abZ - amZ)));
    State(11) = ve + dt*(q0*(q3*(abX - amX) + q0*(abY - amY) - q1*(abZ - amZ)) - gravY - q1*(q1*(abY - amY) - q2*(abX - amX) + q0*(abZ - amZ)) + q2*(q1*(abX - amX) + q2*(abY - amY) + q3*(abZ - amZ)) + q3*(q0*(abX - amX) - q3*(abY - amY) + q2*(abZ - amZ)));
    State(12) = vd + dt*(q0*(q1*(abY - amY) - q2*(abX - amX) + q0*(abZ - amZ)) - gravZ + q1*(q3*(abX - amX) + q0*(abY - amY) - q1*(abZ - amZ)) - q2*(q0*(abX - amX) - q3*(abY - amY) + q2*(abZ - amZ)) + q3*(q1*(abX - amX) + q2*(abY - amY) + q3*(abZ - amZ)));
    State(13) = -abX*(dt*lambdaAccel - 1);
    State(14) = -abY*(dt*lambdaAccel - 1);
    State(15) = -abZ*(dt*lambdaAccel - 1);
}
Eigen::Matrix<double, 16,16> INSEKFfliter::IMUstateTransitionJacobianFcn(MVec3 accelMeas, MVec3 gyroMeas, double dt)
{
    double q0  = State(1-1);
    double q1  = State(2-1);
    double q2  = State(3-1);
    double q3  = State(4-1);
    double gbX = State(5-1);
    double gbY = State(6-1);
    double gbZ = State(7-1);
    double pn  = State(8-1);
    double pe  = State(9-1);
    double pd  = State(10-1);
    double vn  = State(11-1);
    double ve  = State(12-1);
    double vd  = State(13-1);
    double abX = State(14-1);
    double abY = State(15-1);
    double abZ = State(16-1);
    double amX = accelMeas(1-1);
    double amY = accelMeas(2-1);
    double amZ = accelMeas(3-1);
    double gmX = gyroMeas(1-1);
    double gmY = gyroMeas(2-1);
    double gmZ = gyroMeas(3-1);
    double lambdaAccel = 1-accelBiasDecayFactor;
    double lambdaGyro = 1-gyroBiasDecayFactor;

    Eigen::Matrix<double, 16,16> F = Eigen::Matrix<double, 16,16>::Zero(16, 16);
    F(0,0) = 1.0;
    F(0,1) = dt*(gbX/2.0-gmX/2.0);
    F(0,2) = dt*(gbY/2.0-gmY/2.0);
    F(0,3) = dt*(gbZ/2.0-gmZ/2.0);
    F(0,4) = (dt*q1)/2.0;
    F(0,5) = (dt*q2)/2.0;
    F(0,6) = (dt*q3)/2.0;
    F(1,0) = -dt*(gbX/2.0-gmX/2.0);
    F(1,1) = 1.0;
    F(1,2) = -dt*(gbZ/2.0-gmZ/2.0);
    F(1,3) = dt*(gbY/2.0-gmY/2.0);
    F(1,4) = dt*q0*(-1.0/2.0);
    F(1,5) = (dt*q3)/2.0;
    F(1,6) = dt*q2*(-1.0/2.0);
    F(2,0) = -dt*(gbY/2.0-gmY/2.0);
    F(2,1) = dt*(gbZ/2.0-gmZ/2.0);
    F(2,2) = 1.0;
    F(2,3) = -dt*(gbX/2.0-gmX/2.0);
    F(2,4) = dt*q3*(-1.0/2.0);
    F(2,5) = dt*q0*(-1.0/2.0);
    F(2,6) = (dt*q1)/2.0;
    F(3,0) = -dt*(gbZ/2.0-gmZ/2.0);
    F(3,1) = -dt*(gbY/2.0-gmY/2.0);
    F(3,2) = dt*(gbX/2.0-gmX/2.0);
    F(3,3) = 1.0;
    F(3,4) = (dt*q2)/2.0;
    F(3,5) = dt*q1*(-1.0/2.0);
    F(3,6) = dt*q0*(-1.0/2.0);
    F(4,4) = -dt*lambdaGyro+1.0;
    F(5,5) = -dt*lambdaGyro+1.0;
    F(6,6) = -dt*lambdaGyro+1.0;
    F(7,7) = 1.0;
    F(7,10) = dt;
    F(8,8) = 1.0;
    F(8,11) = dt;
    F(9,9) = 1.0;
    F(9,12) = dt;
    F(10,0) = dt*(q0*(abX-amX)*2.0-q3*(abY-amY)*2.0+q2*(abZ-amZ)*2.0);
    F(10,1) = dt*(q1*(abX-amX)*2.0+q2*(abY-amY)*2.0+q3*(abZ-amZ)*2.0);
    F(10,2) = dt*(q2*(abX-amX)*-2.0+q1*(abY-amY)*2.0+q0*(abZ-amZ)*2.0);
    F(10,3) = -dt*(q3*(abX-amX)*2.0+q0*(abY-amY)*2.0-q1*(abZ-amZ)*2.0);
    F(10,10) = 1.0;
    F(10,13) = dt*(q0*q0+q1*q1-q2*q2-q3*q3);
    F(10,14) = -dt*(q0*q3*2.0-q1*q2*2.0);
    F(10,15) = dt*(q0*q2*2.0+q1*q3*2.0);
    F(11,0) = dt*(q3*(abX-amX)*2.0+q0*(abY-amY)*2.0-q1*(abZ-amZ)*2.0);
    F(11,1) = -dt*(q2*(abX-amX)*-2.0+q1*(abY-amY)*2.0+q0*(abZ-amZ)*2.0);
    F(11,2) = dt*(q1*(abX-amX)*2.0+q2*(abY-amY)*2.0+q3*(abZ-amZ)*2.0);
    F(11,3) = dt*(q0*(abX-amX)*2.0-q3*(abY-amY)*2.0+q2*(abZ-amZ)*2.0);
    F(11,11) = 1.0;
    F(11,13) = dt*(q0*q3*2.0+q1*q2*2.0);
    F(11,14) = dt*(q0*q0-q1*q1+q2*q2-q3*q3);
    F(11,15) = -dt*(q0*q1*2.0-q2*q3*2.0);
    F(12,0) = dt*(q2*(abX-amX)*-2.0+q1*(abY-amY)*2.0+q0*(abZ-amZ)*2.0);
    F(12,1) = dt*(q3*(abX-amX)*2.0+q0*(abY-amY)*2.0-q1*(abZ-amZ)*2.0);
    F(12,2) = -dt*(q0*(abX-amX)*2.0-q3*(abY-amY)*2.0+q2*(abZ-amZ)*2.0);
    F(12,3) = dt*(q1*(abX-amX)*2.0+q2*(abY-amY)*2.0+q3*(abZ-amZ)*2.0);
    F(12,12) = 1.0;
    F(12,13) = -dt*(q0*q2*2.0-q1*q3*2.0);
    F(12,14) = dt*(q0*q1*2.0+q2*q3*2.0);
    F(12,15) = dt*(q0*q0-q1*q1-q2*q2+q3*q3);
    F(13,13) = -dt*lambdaAccel+1.0;
    F(14,14) = -dt*lambdaAccel+1.0;
    F(15,15) = -dt*lambdaAccel+1.0;
    return F;
}
Eigen::Matrix<double, 16,12> INSEKFfliter::IMUnoiseJacobianFcn(Eigen::Matrix<double, 16,1>& x, double dt)
{
    double q0  = x(1-1);
    double q1  = x(2-1);
    double q2  = x(3-1);
    double q3  = x(4-1);
    double gbX = x(5-1);
    double gbY = x(6-1);
    double gbZ = x(7-1);
    double pn  = x(8-1);
    double pe  = x(9-1);
    double pd  = x(10-1);
    double vn  = x(11-1);
    double ve  = x(12-1);
    double vd  = x(13-1);
    double abX = x(14-1);
    double abY = x(15-1);
    double abZ = x(16-1);
    Eigen::Matrix<double, 16,12> G = Eigen::Matrix<double, 16,12>::Zero(16, 12);
    G(0,0) = dt*q1*(-1.0/2.0);
    G(0,1) = dt*q2*(-1.0/2.0);
    G(0,2) = dt*q3*(-1.0/2.0);
    G(1,0) = (dt*q0)/2.0;
    G(1,1) = dt*q3*(-1.0/2.0);
    G(1,2) = (dt*q2)/2.0;
    G(2,0) = (dt*q3)/2.0;
    G(2,1) = (dt*q0)/2.0;
    G(2,2) = dt*q1*(-1.0/2.0);
    G(3,0) = dt*q2*(-1.0/2.0);
    G(3,1) = (dt*q1)/2.0;
    G(3,2) = (dt*q0)/2.0;
    G(10,6) = -dt*(q0*q0+q1*q1-q2*q2-q3*q3);
    G(10,7) = dt*(q0*q3*2.0-q1*q2*2.0);
    G(10,8) = -dt*(q0*q2*2.0+q1*q3*2.0);
    G(11,6) = -dt*(q0*q3*2.0+q1*q2*2.0);
    G(11,7) = -dt*(q0*q0-q1*q1+q2*q2-q3*q3);
    G(11,8) = dt*(q0*q1*2.0-q2*q3*2.0);
    G(12,6) = dt*(q0*q2*2.0-q1*q3*2.0);
    G(12,7) = -dt*(q0*q1*2.0+q2*q3*2.0);
    G(12,8) = -dt*(q0*q0-q1*q1-q2*q2+q3*q3);
    return G;
}
Eigen::Matrix<double, 12,12> INSEKFfliter::IMUnoiseCovariance()
{
    Eigen::Matrix<double, 12,12> U = Eigen::Matrix<double, 12,12>::Zero(12, 12);
    U(0,0)   = GyroscopeNoise;
    U(1,1)   = GyroscopeNoise;
    U(2,2)   = GyroscopeNoise;
    U(3,3)   = GyroscopeBiasNoise;
    U(4,4)   = GyroscopeBiasNoise;
    U(5,5)   = GyroscopeBiasNoise;
    U(6,6)   = AccelerometerNoise;
    U(7,7)   = AccelerometerNoise;
    U(8,8)   = AccelerometerNoise;
    U(9,9)   = AccelerometerBiasNoise;
    U(10,10) = AccelerometerBiasNoise;
    U(11,11) = AccelerometerBiasNoise;
    return U;
}
void INSEKFfliter::predictCovEqnFcn(Eigen::Matrix<double, 16,16>& F,Eigen::Matrix<double, 12,12>& U, Eigen::Matrix<double, 16,12>& G)
{
    Eigen::Matrix<double, 16,16> Q = G*U*(G.transpose());
    StateCovariance = F*StateCovariance*(F.transpose()) + Q;
}
Eigen::Matrix<double, 3,1> INSEKFfliter::GPSPositionmeasurementFcn(Eigen::Matrix<double, 16,1>& x)
{
    Eigen::Matrix<double, 3,1> pos;
    pos(0) = x(7);
    pos(1) = x(8);
    pos(2) = x(9);
    return pos;
}
Eigen::Matrix<double, 3,16> INSEKFfliter::GPSPositionmeasurementJacobianFcn(Eigen::Matrix<double, 16,1>& x)
{
    Eigen::Matrix<double, 3,16> H = Eigen::Matrix<double, 3,16>::Zero(3, 16);
    H(0, 7) = 1;
    H(1, 8) = 1;
    H(2, 9) = 1;
    return H;
}
MVec3 INSEKFfliter::lla2enu(double lon, double lat)
{
    MVec3 pos;
    double pi = 3.1415926;
    double x = lon*20037508.342789/180;
    double y = log(tan((90+lat)*pi/360))/(pi/180)*20037508.342789/180;
    pos(0) = x;
    pos(1) = y;
    pos(2) = 0;
    return pos;
}
void INSEKFfliter::correctEqnFcn(Eigen::Matrix<double, 3,1>& h, Eigen::Matrix<double, 3,16>& H, const MVec3& pos, Eigen::Matrix<double, 3,3>& R)
{
    auto innovCov = H*StateCovariance*(H.transpose()) + R;
    auto W = StateCovariance*(H.transpose()) *innovCov.inverse();
    State = State + W*(pos - h);
    StateCovariance = StateCovariance - W*H*StateCovariance;
}