欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

多传感器融合定位(4-基于滤波的2融合方法)2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析

程序员文章站 2022-03-04 17:26:03
...

多传感器融合定位(4-基于滤波的2融合方法)2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析

本次作业摘自 张松鹏大哥的优秀作业
代码下载 https://github.com/rainbowrooster/Multi—sensor-fusion-and-positioning/tree/main/chapter4/Q2
下载说明:chapter4/Q2 中 , gnss-ins-sim 优秀代码为鹏哥自定义仿真场景,和使用gnss-ins-sim生成仿真IMU数据的py文件;ins里 ins.cpp 为 鹏哥的eskf 模型和 可观性分析源码,eskf_ins.cpp 为参鹏哥代码自打代码,包含自己理解的注解;matplolib 里 plot_data.m 为修改后适应matlab2016a的可视化代码,plot_data2.m为鹏哥的原可视化代码。

主要公式:

kalman 五大重要公式

多传感器融合定位(4-基于滤波的2融合方法)2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析

姿态误差、速度误差、位置误差

多传感器融合定位(4-基于滤波的2融合方法)2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析
多传感器融合定位(4-基于滤波的2融合方法)2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析

eskf 状态方程

多传感器融合定位(4-基于滤波的2融合方法)2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析
多传感器融合定位(4-基于滤波的2融合方法)2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析

eskf 观测方程

多传感器融合定位(4-基于滤波的2融合方法)2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析

离散时间滤波器

多传感器融合定位(4-基于滤波的2融合方法)2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析
多传感器融合定位(4-基于滤波的2融合方法)2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析

部分代码片 eskf_ins.cpp:

同步GPS 和 IMU 数据

在同步数据中,分为 未初始化传感器数据同步(IMU数据第一帧) 和 初始化传感器后的数据同步(多帧IMU数据) ;时间同步以gnss数据为准。

以gnss数据为同步基准

    /*以gnss时间为准*/
    if (gnss_buff.empty())
    {
        return false;
    }
    current_gnss = gnss_buff.front();   //读取当前gnss队列第一个数据
    double sync_time = current_gnss.time;

GroundTruth 时间 数据 同步

/*sync groundtruth 数据*/
//gt时间同步
    while (gt_buff.size() > 1)
    {
        if(gt_buff[1].time < sync_time)
        {
            gt_buff.pop_front();       //将不对齐的时间删除
        }else
        {
            break;
        }
    }
//gt数据同步
if(gt_buff.size() > 1)
{   
    PoseData front_data = gt_buff.at(0);    //上一帧数据
    PoseData back_data = gt_buff.at(1);
    double front_scale = (back_data.time - sync_time) / (back_data.time - front_data.time); 
    double back_scale = (sync_time -front_data.time) /  (back_data.time - front_data.time);
    current_gt.time = sync_time ;
    //插值
    current_gt.state.p = front_data.state.p  * front_scale  + back_data.state.p *  back_scale;
    current_gt.state.v = front_data.state.v   * front_scale  + back_data.state.v *  back_scale;
    current_gt.state.q= front_data.state.q.slerp(front_scale,back_data.state.q);
    current_gt.state.bg = front_data.state.bg *  front_scale  + back_data.state.bg * back_scale;
    current_gt.state.ba = front_data.state.ba * front_scale   + back_data.state.ba * back_scale;
}else{
    return false;
}

IMU 时间 数据 同步

// imu 数据同步
if (imu_buff.size() > 1)
{
    /*IMU 未初始化的数据同步*/
    if (!inited)
    {
        current_imu.clear();
        IMUData front_data = imu_buff.at(0);
        IMUData back_data = imu_buff.at(1);
        IMUData synced_data;

        double front_scale = (back_data.time - sync_time) / (back_data.time - front_data.time);
        double back_scale = (sync_time - front_data.time) / (back_data.time - front_data.time);
        synced_data.time = sync_time;
        synced_data.acc = front_data.acc * front_scale + back_data.acc * back_scale;
        synced_data.gyro = front_data.gyro * front_scale + back_data.gyro * back_scale;
        current_imu.push_back(synced_data);
        imu_buff.pop_front();
        gnss_buff.pop_front();
        // std::cout << std::setprecision(12) << "sync_time " << sync_time
        //           << " current_imu.time " << current_imu.front().time
        //           << "  " << current_imu.back().time << std::endl;
        return true;
    }

    if (imu_buff.back().time < sync_time)
    {
        return false;
    }

        while (current_imu.size() > 1)
    {
        current_imu.pop_front();
    }

    while (imu_buff.front().time < sync_time)
    {
        IMUData temp = imu_buff.front();
        imu_buff.pop_front();
        current_imu.push_back(temp);     // 将新的IMU数据放在deque队列末尾
    }
    IMUData front_data = current_imu.back();
    IMUData back_data = imu_buff.at(0);      //取 刚好比 sync_time 大的imu 数据
    IMUData synced_data;
    //插值
    double front_scale = (back_data.time - sync_time) / (back_data.time - front_data.time);
    double back_scale = (sync_time - front_data.time) / (back_data.time - front_data.time);
    synced_data.time = sync_time;
    synced_data.acc = front_data.acc * front_scale  +  back_data.acc  * back_scale; 
    synced_data.gyro = front_data.gyro * front_scale  + back_data.gyro * back_scale;
    current_imu.push_back(synced_data);

    gnss_buff.pop_front();
    return true;
}else{
    return false;
    }

传感器初始化

bool InitSensor()          //初始化 传感器
{
    while (!gnss_buff.empty())
    {
         if (imu_buff.front().time >  gnss_buff.front().time)
         {
              gnss_buff.pop_front();       //删除时间不匹配的 gnss 数据,保证 imu的数据在前
         }else
         {
             return true;
         }
    }
    return  false;
}

位姿初始化

bool InitPose()
{
    static bool pose_inited = false;
    if (pose_inited)
    {
        return true;
    }
    if (!SyncData(false))
    {
        return false;
    }
    current_pose.time = current_gt.time;
    current_pose.state.p = current_gt.state.p;
    current_pose.state.q = current_gt.state.q;
    current_pose.state.v = current_gt.state.v;
    current_pose.state.bg = current_gt.state.bg;
    current_pose.state.ba = current_gt.state.ba;
    current_error_state.x.setZero();
    current_error_state.p.setZero();
    current_error_state.p.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity() * init_noise[0];
    current_error_state.p.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity() * init_noise[1];
    current_error_state.p.block<3, 3>(6, 6) = Eigen::Matrix3d::Identity() * init_noise[2];
    current_error_state.p.block<3, 3>(9, 9) = Eigen::Matrix3d::Identity() * init_noise[3];
    current_error_state.p.block<3, 3>(12, 12) = Eigen::Matrix3d::Identity() * init_noise[4];
    pose_inited = true;
    return true;
}

基于误差的卡尔曼滤波

/*滤波*/
bool Filter()
{
    Predict();
    if (correct)
    {
        Correct();
    }
    return true;
}

eskf 预测

bool Predict()  // 预测
{
    current_pose.time  = current_gt.time;
    Eigen::Vector3d  pp =  current_pose.state.p;
    Eigen::Vector3d  vv  =  current_pose.state.v;
    Eigen::Quaterniond qq = current_pose.state.q;
    double w = 7.27220521664304e-05;    // 地球自转速度
    Eigen::Vector3d gn(0, 0, -9.79484197226504);    // 重力加速度
    Eigen::Vector3d w_ie_n(0, w * std::cos(current_gnss.lla[0] * M_PI / 180),
                           w * std::sin(current_gnss.lla[0] * M_PI / 180));
    double rm = 6353346.18315;       // 短半轴
    double rn = 6384140.52699;        // 长半轴
    Eigen::Vector3d w_en_n(-vv[1] / (rm + current_gnss.lla[2]),
                           vv[0] / (rn + current_gnss.lla[2]),
                           vv[0] / (rn + current_gnss.lla[2]) * std::tan(current_gnss.lla[0] * M_PI / 180));
    Eigen::Vector3d w_in_n = w_ie_n + w_en_n;
    for (int i = 1; i < current_imu.size(); ++i)
    {
        double dt = current_imu[i].time - current_imu[i - 1].time;
        Eigen::Vector3d wtemp = w_in_n * dt;
        double angle = wtemp.norm();
        Eigen::Quaterniond qn(1, 0, 0, 0);
        if (angle != 0)
        {
            wtemp = wtemp / angle;
            wtemp = std::sin(angle / 2) * wtemp;
            qn = Eigen::Quaterniond(std::cos(angle / 2), wtemp[0], wtemp[1], wtemp[2]);
        }
        qn.normalize();   // 地球自转的角增量

    Eigen::Vector3d wb = 0.5* current_imu[i - 1].gyro  + 0.5*current_imu[i].gyro;
    wb = wb + current_pose.state.bg;
    wb = wb * dt;
    angle = wb.norm();
    Eigen::Quaterniond qb(1,0,0,0);
    if (angle != 0)
    {
        wb = wb / angle;
        wb = std::sin(angle / 2) * wb;
        qb = Eigen::Quaterniond(std::cos(angle / 2),wb[0],wb[1],wb[2] );
    }
    qb.normalize();     //载体角增量

    Eigen::Quaterniond  qq2 = qn.inverse() * qq * qb;   // 下一时刻的旋转矩阵
    Eigen::Vector3d f1 = current_imu[i-1].acc;
    f1 = f1 + current_pose.state.ba;
    Eigen::Vector3d f2 = current_imu[i].acc;
    f2 = f2 + current_pose.state.ba;
    Eigen::Vector3d vv2 = vv + dt * (0.5* (qq * f1 + qq * f2) + gn);  //下一时刻的速度
    Eigen::Vector3d pp2 = pp + 0.5*dt* (vv + vv2);
    pp = pp2;
    vv  = vv2;
    qq = qq2;
}   
current_pose.state.p = pp;    //更新当前位置
current_pose.state.v =  vv;
current_pose.state.q = qq;

Ft = Eigen::Matrix<double,15,15>::Zero();    // 状态转移矩阵
Ft.block<3,3>(0,3) = Eigen::Matrix<double, 3 ,3>::Identity();
Eigen::Matrix<double,3,3>temp = Eigen::Matrix<double,3,3>::Zero();
Eigen::Vector3d ff = current_imu.back().acc;
// F23
ff = qq*ff;
temp(0,1) = -ff[2];
temp(0,2) = ff[1];
temp(1,0) = ff[2];
temp(1, 2) = -ff[0];
temp(2, 0) = -ff[1];
temp(2, 1) = ff[0];
Ft.block<3,3>(3,6) = temp;
// Cb_n
Ft.block<3,3>(3,12) = qq.toRotationMatrix();
temp.setZero();
// F33 
temp(0, 1) = w_ie_n(2);
temp(0, 2) = -w_ie_n(1);
temp(1, 0) = -w_ie_n(2);
temp(2, 0) = w_ie_n(1);
Ft.block<3,3>(6,6)  = temp;
//- Cb_n
Ft.block<3,3>(6,9) =  -Ft.block<3,3>(3,12);
Eigen::Matrix<double,15,6>  Bt = Eigen::Matrix<double,15,6>::Zero();
Bt.block<3,3>(3,3) = Ft.block<3,3>(3,12);
Bt.block<3,3>(6,0) = Ft.block<3, 3>(6, 9);
T = current_imu.back().time  - current_imu.front().time; 
//   Qso 离散滤波器
Ft = Eigen::Matrix<double,15 ,15>::Identity() + Ft * T ;      // Fk-1 ,上一时刻的状态转移矩阵
Bt = Bt * T ;                                                                                           // Bk-1 ,上一时刻的观测矩阵
Eigen::Matrix<double,6,1> W = Eigen::Matrix<double,6,1>::Zero();    //器件噪声 ,一般指 IMU的零偏不稳定系
// eskf 先验
current_error_state.x   = Ft * current_error_state.x  + Bt * W;   
Eigen::Matrix<double, 6, 6> Q = Eigen::Matrix<double, 6, 6>::Identity();                //  惯性噪声
Q.block<3,3>(0,0) = Eigen::Matrix<double,3,3>:: Identity()* gyro_noise * gyro_noise;
Q.block<3,3>(3,3) = Eigen::Matrix<double,3,3>:: Identity()* acc_noise  * acc_noise;
current_error_state.p = Ft * current_error_state.p * Ft.transpose() + Bt * Q * Bt.transpose();
return  true ;
}

eskf 修正

// 修正融合 
bool Correct()
{
    double geo_x, geo_y, geo_z;
    geo_converter.Forward(current_gnss.lla(0), current_gnss.lla(1),
                          current_gnss.lla(2), geo_x, geo_y, geo_z);      // 将经纬度处理为  x y z
    Eigen::Vector3d gnss_xyz(geo_x, geo_y, geo_z);

    Y.block<3,1>(0,0) = current_pose.state.p -  gnss_xyz;  // 观测误差

    Gt = Eigen::Matrix<double,3,15>::Zero();
    Gt.block<3,3>(0,0) = Eigen::Matrix<double,3,3>::Identity();
    Eigen::Matrix<double,3,3> Ct = Eigen::Matrix<double,3,3>::Identity();

    Eigen::Matrix<double, 3, 3> R = Eigen::Matrix<double, 3, 3>::Identity();    // 观测噪声, gps噪声
    R.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity() * dp_noise * dp_noise;
    Eigen::Matrix<double, 15, 3> K = current_error_state.p * Gt.transpose() * (Gt * current_error_state.p * Gt.transpose() + Ct * R * Ct.transpose()).inverse();  // kf 增益

    // 计算后验
    current_error_state.p = (Eigen::Matrix<double, 15, 15>::Identity() - K * Gt) * current_error_state.p;     // 后验方差
    current_error_state.x = current_error_state.x + K * (Y - Gt * current_error_state.x);                                      // 后验状态
    // 位姿、姿态 update 
    current_pose.state.p = current_pose.state.p - current_error_state.x.block<3,1>(0,0);
    current_pose.state.v = current_pose.state.v - current_error_state.x.block<3,1>(3,0);
         Eigen::Vector3d dphi_dir = current_error_state.x.block<3, 1>(6, 0);
    double dphi_norm = dphi_dir.norm();
    if (dphi_norm != 0)
    {
        dphi_dir = dphi_dir / dphi_norm;
        dphi_dir = dphi_dir * std::sin(dphi_norm / 2);
    }
    Eigen::Quaterniond temp2(std::cos(dphi_norm / 2), dphi_dir[0], dphi_dir[1], dphi_dir[2]);
    current_pose.state.q = temp2 * current_pose.state.q;     // 更新旋转矢量
    current_pose.state.q.normalize();
    current_pose.state.bg = current_pose.state.bg - current_error_state.x.block<3, 1>(9, 0);
    current_pose.state.ba = current_pose.state.ba - current_error_state.x.block<3, 1>(12, 0);
    current_error_state.x.setZero();        // 清空误差
    return true;
}

保存观测度分析所需的F G 和 Y

/*保存观测度分析所需的F G 和 Y*/
bool SaveFG()    
{
    if (FGs.size() > FGsize)
    {
        return true;
    }
    if (FGs.empty())
    {
        FG fg;
        fg.time = current_gt.time;
        // fg.F = Ft;
        fg.F = Ft - Eigen::Matrix<double, 15, 15>::Identity();
        // fg.F = (Ft - Eigen::Matrix<double, 15, 15>::Identity()) / T;
        fg.G = Gt;
        fg.Y.push_back(Y);
        FGs.push_back(fg);
    }
    else
    {
        if (FGs.back().Y.size() == 15)
        {
            if (current_gt.time - FGs.back().time < time_interval || FGs.size() >= FGsize)
            {
                return true;
            }
            FG fg;
            fg.time = current_gt.time;
            // fg.F = Ft;
            fg.F = Ft - Eigen::Matrix<double, 15, 15>::Identity();
            // fg.F = (Ft - Eigen::Matrix<double, 15, 15>::Identity()) / T;
            fg.G = Gt;
            fg.Y.push_back(Y);
            FGs.push_back(fg);
        }
        else
        {
            FGs.back().Y.push_back(Y);
        }
        
    }
    return true;
}

保存位姿信息

void SavePose(std::ofstream &save_points, PoseData &pose)
{
    Eigen::Quaterniond qtemp = pose.state.q;
    // if (qtemp.w() < 0)
    // {
    //     qtemp.coeffs() = -1.0 * qtemp.coeffs();
    // }
    double angle = std::acos(qtemp.w()) * 2;
    double sin_angle = std::sin(angle / 2);
    Eigen::Vector3d dir(0, 0, 0);
    if (sin_angle != 0)
    {
        dir(0) = qtemp.x() / sin_angle;
        dir(1) = qtemp.y() / sin_angle;
        dir(2) = qtemp.z() / sin_angle;
        dir = dir * angle;
    }
    save_points.precision(12);
    save_points << pose.time
                << "," << pose.state.p(0)
                << "," << pose.state.p(1)
                << "," << pose.state.p(2)
                << "," << pose.state.v(0)
                << "," << pose.state.v(1)
                << "," << pose.state.v(2)
                // << "," << pose.state.q.x()
                // << "," << pose.state.q.y()
                // << "," << pose.state.q.z()
                // << "," << pose.state.q.w()
                << "," << dir(0)
                << "," << dir(1)
                << "," << dir(2)
                << "," << pose.state.bg(0)
                << "," << pose.state.bg(1)
                << "," << pose.state.bg(2)
                << "," << pose.state.ba(0)
                << "," << pose.state.ba(1)
                << "," << pose.state.ba(2)
                << std::endl;
}

可观测性分析

    Eigen::MatrixXd  Qso(3*15*FGs.size(),15);
    Eigen::MatrixXd  Ys(3*15*FGs.size(),1);
    for (int i = 0; i < FGs.size(); ++i)
    {
        Eigen::Matrix<double,15,15> Fn = Eigen::Matrix<double,15,15>::Identity();
        for (int j = 0; j < 15; j++)
        {
            if (j > 0)
            {
                Fn = Fn * FGs[i].F;
            }
            Ys.block<3, 1>(i*3*15+3*j, 0) = FGs[i].Y[j];
            Qso.block<3, 15>(i*3*15+3*j, 0) = FGs[i].G * Fn ;    // 构建可观测性矩阵
        }
    }
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(Qso, Eigen::ComputeFullU | Eigen::ComputeFullV);
    std::cout << Qso.rows() << ", " << Qso.cols() << std::endl;
    // std::cout << svd.singularValues() << std::endl;
    for (int i = 0; i < 15; ++i)
    {
        double temp = (svd.matrixU().row(i) * Ys)[0] / svd.singularValues()[i];
        Eigen::MatrixXd Xi = temp * svd.matrixV().col(i);
        // std::cout << Xi.transpose() << std::endl;
        Eigen::MatrixXd::Index maxRow, maxCol;
        Xi = Xi.cwiseAbs();   //  Xi的绝对值
        double maxvalue = Xi.maxCoeff(&maxRow, &maxCol);
     std::cout << svd.singularValues()(i) / svd.singularValues()(0) << "," << maxRow << std::endl;
        sv_ofs << svd.singularValues()(i) / svd.singularValues()(0) << "," << maxRow << std::endl;
    }

仿真与分析

采用 imu_data7.py 生成数据

    # imu_err
    imu_err = {'gyro_b': np.array([1e-2, 2e-2, 3e-2]) / D2R * 3600 * 0,
               'gyro_arw': np.array([1e-5, 1e-5, 1e-5]) / D2R * 60 * 0,
               'gyro_b_stability': np.array([1e-5, 1e-5, 1e-5]) / D2R * 3600 * 1e-0,
                  'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
               'accel_b': np.array([2.0e-3, 1.0e-3, 5.0e-3]) * 0,
               'accel_vrw': np.array([1e-4, 1e-4, 1e-4]) * 60.0 * 0,
               'accel_b_stability': np.array([1e-4, 1e-4, 1e-4]) * 1.0 * 1e0,
                  'accel_b_corr': np.array([200.0, 200.0, 200.0]),
               #    'mag_std': np.array([0.2, 0.2, 0.2]) * 1.0
               }
    # generate GPS and magnetometer data
    gps_err = {
        'stdp': np.array([1, 1, 1]) * 1e-6,
        'stdv': np.array([0, 0, 0]),
    }
    imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=True, gps_opt=gps_err)

其中陀螺仪零偏稳定性为 1e-5rad/s(约 2deg/h),加速度计零偏稳定性为 1e-4m/s^2,GPS 位置误
差为 1e-6m/s,其它误差量均设置为 0。运行 ins 生成数据后,采用 plot_data.m 来绘制各状态量和真实
值的数据,采用 plot_data2.m 来绘制各状态量与真实值相减之后的数据,以分析收敛情况,具体代码见
附件。在可观测度分析中,各状态量对应的编号从 0 到 14。

运行 ./ins 或 ./eskf_ins 对 四种场景进行可观测性分析 eg. 结果总结部分摘自张松鹏优秀作业讲解
a) 静止状态
如下图终端打印显示为,生成的 在时变系统下 Qso矩阵的 450行,15列; 第二行-第16行为对Qso矩阵进行SVD分解后得出的降序排列奇异值和对应的状态量,可观测度一般低于 1e-4 或 1e-5 认为不可观。
多传感器融合定位(4-基于滤波的2融合方法)2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析
运行 plot_data.m 生成可视化分析
各状态量的收敛情况见下图,从图中可看出,航向角(8),而这这个状态量对应的可观测度系数均非常小,为e-05量级,另外,z 轴角速度零偏的收敛速度很慢,直到 10分钟后才逐渐收敛,它对应的可观测度也很低,在 1.2e-7 左右,由于其仍然能够收敛,因此认为它是可观的。
多传感器融合定位(4-基于滤波的2融合方法)2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析
通调整 FGsize,加大时变系统下的方程组数,可使变量可观
多传感器融合定位(4-基于滤波的2融合方法)2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析
b) 匀速状态 -摘自 松鹏大哥作业
仿真命令参见附件文件 imu_def8.csv,b 系沿前向(y 轴正向)做匀速运动。各状态量的收敛情况见下图,可观测度系数见下表。从图中可看出,收敛情况与静止状态完全相同,航向角,x 轴加速度零偏和 y 轴加速度零偏均未收敛,这 3 个状态量对应的可观测度系数均很小,在 1e-33 到 1e-28 量级,因此 3 个状态量是不可观的。z 轴角速度零偏收敛速度很慢,对应的可观测度也很低,在 6e-8 左右,但仍然能够收敛,因此是可观的。
多传感器融合定位(4-基于滤波的2融合方法)2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析
多传感器融合定位(4-基于滤波的2融合方法)2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析
c) 加减速状态 -摘自 松鹏大哥作业
仿真命令参见附件文件 imu_def9.csv,b 系沿 x 轴和 y 轴两个方向分别做加减速运动,具体运动参考下图中的位置和速度变化曲线。可观测度系数见下表。各状态量对应的可观测度系数均在 7.9e-4 以上,而图中各状态量的误差值均在 0 值附近波动,可认为达到收敛状态。但由于 IMU 数据存在误差,各状态量均存在一定程度的误差。在加减速变换时,角度和零偏的误差值均有一定波动。可观测度系数中最小的 3 个是 z 轴角速度零偏,x 轴加速度零偏,y 轴加速度零偏,均在 1e-3 量级,3 个状态量观测度较低,而收敛
速度也较慢,特别是 2 个加速度水平零偏,在 20s 之后误差值才逐渐减小,因此对应的可观测度也最低。航向角的误差值在几次加减速后逐渐下降了一个数量级,对应的可观测度较前面 2 种情况也有大幅提高,约 0.012。

多传感器融合定位(4-基于滤波的2融合方法)2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析
多传感器融合定位(4-基于滤波的2融合方法)2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析
d) 转向状态 -摘自 松鹏大哥作业
仿真命令参见附件文件 imu_def10.csv,b 系做绕 8 字运动,具体运动情况见下图的位置和姿态变化。各状态量可观测度系数见下表。各状态量对应的可观测度系数均在 8.7e-4 以上,而图中各状态量的误差值均在 0 值附近波动,可认为达到收敛状态。但由于 IMU 数据存在误差,各状态量均存在一定程度的误差。在做转向变换时,角度和零偏的误差值均有一定波动。可观测度系数小于或者等于 0.01 量级的有 7 个状态量,包括航向角和 6 个零偏参数,这 7 个参数均在开始有所波动,但经过几十秒后收敛。
多传感器融合定位(4-基于滤波的2融合方法)2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析
多传感器融合定位(4-基于滤波的2融合方法)2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析
多传感器融合定位(4-基于滤波的2融合方法)2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析

添加个人注解的 ins.cpp

/*eskf_ins.cpp*/
#include <vector>
#include <Eigen/Core>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <Eigen/Geometry>
#include "Geocentric/LocalCartesian.hpp"
#include <deque>
#include <random>
#include <yaml-cpp/yaml.h>

#define D2R  0.017453292519943295       // Degree2Radius
struct  State
{
    Eigen::Vector3d  p ;          //位置
    Eigen::Vector3d  v;           //速度
    Eigen::Quaterniond q;   //位姿
    Eigen::Vector3d bg ;       // bias-gyro
    Eigen::Vector3d ba;       //  bias-accel
};
//eskf状态方程变量
struct  ErrorState
{
    Eigen::Matrix<double,15,1> x;    //状态
    Eigen::Matrix<double,15,15> p ;    // 方差
};
struct  IMUData
{
    double time;
    Eigen::Vector3d acc;
    Eigen::Vector3d gyro;
};
struct  GNSSData
{
    double  time;
    Eigen::Vector3d lla;   //经纬度
    Eigen::Vector3d v;      //速度
};
struct  PoseData
{
    double  time;
    State state;
};

std::deque<GNSSData>  gnss_buff;
std::deque<IMUData> imu_buff;
std::deque<PoseData> gt_buff;
std::deque<IMUData> current_imu;
GNSSData  current_gnss;
PoseData   current_gt;
PoseData   current_pose;
ErrorState  current_error_state;
GeographicLib::LocalCartesian geo_converter(32, 120, 0);    // 初始经纬度
std::ofstream    gt_ofs;
std::ofstream    pose_ofs;
std::ofstream    sv_ofs;
double gyro_noise  =  1e-6;
double acc_noise = 1e-5;
double  dp_noise = 1e-3;
std::vector<double>  init_noise;
int FGsize = 20;     //Qso  20个时刻
double time_interval = 10;
double end_time = 20;
double T = 0.1;
struct  FG
{
    double time;
    Eigen::Matrix<double,15,15> F;                         // 状态转移矩阵
    Eigen::Matrix<double,3,15> G;                          //观测矩阵
    std::vector<Eigen::Matrix<double,3,1>> Y;   //观测值
};
std::vector<FG> FGs;                                                  //多个时刻的 FG矩阵
//当前时刻的  F  G  Y 矩阵
Eigen::Matrix<double,15,15> Ft = Eigen::Matrix<double,15,15>::Zero(); 
Eigen::Matrix<double,3,15>  Gt = Eigen::Matrix<double,3,15>::Zero();
Eigen::Matrix<double,3,1> Y  = Eigen::Matrix<double,3,1>::Zero();
bool correct = true;

//读取仿真数据
//   读取仿真数据
bool ReadData(const std::vector<std::string> &path)
{
    gnss_buff.clear();
    imu_buff.clear();
    gt_buff.clear();
    std::vector<std::ifstream> reads;
    // int count = 0;
    for (int i = 0; i < path.size(); ++i)
    {
        reads.push_back(std::ifstream(path[i]));
    }
    for (int i = 0; i < path.size(); ++i)
    {
        std::string strs;
        std::getline(reads[i], strs);
    }
    std::string strs;
    while (std::getline(reads[0], strs))
    {
        double time = std::stod(strs);
        std::getline(reads[1], strs);
        std::string temp = "";
        std::vector<double> acc;
        for (int i = 0; i < strs.size(); ++i)
        {
            if (strs[i] == ',')
            {
                acc.push_back(std::stod(temp));
                temp = "";
            }
            else
            {
                temp = temp + strs[i];
            }
        }
        acc.push_back(std::stod(temp));

        std::getline(reads[2], strs);
        temp = "";
        std::vector<double> gyro;
        for (int i = 0; i < strs.size(); ++i)
        {
            if (strs[i] == ',')
            {
                gyro.push_back(std::stod(temp));
                temp = "";
            }
            else
            {
                temp = temp + strs[i];
            }
        }
        gyro.push_back(std::stod(temp));
        IMUData imu;
        imu.time = time;
        imu.acc = Eigen::Vector3d(acc[0], acc[1], acc[2]);
        imu.gyro = Eigen::Vector3d(gyro[0] * D2R, gyro[1] * D2R, gyro[2] * D2R);
        imu_buff.push_back(imu);

        std::getline(reads[5], strs);
        temp = "";
        std::vector<double> ref_pos;
        for (int i = 0; i < strs.size(); ++i)
        {
            if (strs[i] == ',')
            {
                ref_pos.push_back(std::stod(temp));
                temp = "";
            }
            else
            {
                temp = temp + strs[i];
            }
        }
        ref_pos.push_back(std::stod(temp));

        std::getline(reads[6], strs);
        temp = "";
        std::vector<double> ref_vel;
        for (int i = 0; i < strs.size(); ++i)
        {
            if (strs[i] == ',')
            {
                ref_vel.push_back(std::stod(temp));
                temp = "";
            }
            else
            {
                temp = temp + strs[i];
            }
        }
        ref_vel.push_back(std::stod(temp));

        std::getline(reads[7], strs);
        temp = "";
        std::vector<double> ref_att_quat;
        for (int i = 0; i < strs.size(); ++i)
        {
            if (strs[i] == ',')
            {
                ref_att_quat.push_back(std::stod(temp));
                temp = "";
            }
            else
            {
                temp = temp + strs[i];
            }
        }
        ref_att_quat.push_back(std::stod(temp));

        Eigen::Quaterniond q = Eigen::AngleAxisd(90 * D2R, Eigen::Vector3d::UnitZ()) *
                               Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) *
                               Eigen::AngleAxisd(180 * D2R, Eigen::Vector3d::UnitX());
        q = q.inverse();

        PoseData pose;
        pose.time = time;
        double geo_x, geo_y, geo_z;
        geo_converter.Forward(ref_pos[0], ref_pos[1], ref_pos[2], geo_x, geo_y, geo_z);
        pose.state.p = Eigen::Vector3d(geo_x, geo_y, geo_z);
        // pose.state.p = q * Eigen::Vector3d(ref_pos[0], ref_pos[1], ref_pos[2]);
        pose.state.v = q * Eigen::Vector3d(ref_vel[0], ref_vel[1], ref_vel[2]);
        pose.state.q = q * Eigen::Quaterniond(ref_att_quat[0], ref_att_quat[1], ref_att_quat[2], ref_att_quat[3]);
        pose.state.q.normalize();
        pose.state.bg = Eigen::Vector3d(0, 0, 0);
        pose.state.ba = Eigen::Vector3d(0, 0, 0);
        gt_buff.push_back(pose);
    }
    while (std::getline(reads[3], strs))
    {
        double time = std::stod(strs);
        std::getline(reads[4], strs);
        std::string temp = "";
        std::vector<double> gps;
        for (int i = 0; i < strs.size(); ++i)
        {
            if (strs[i] == ',')
            {
                gps.push_back(std::stod(temp));
                temp = "";
            }
            else
            {
                temp = temp + strs[i];
            }
        }
        gps.push_back(std::stod(temp));
        GNSSData gnss;
        gnss.time = time;
        gnss.lla = Eigen::Vector3d(gps[0], gps[1], gps[2]);
        // 北东地   ->   东北天
        Eigen::Quaterniond q = Eigen::AngleAxisd(90 * D2R, Eigen::Vector3d::UnitZ()) *
                               Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) *
                               Eigen::AngleAxisd(180 * D2R, Eigen::Vector3d::UnitX());
        q = q.inverse();
        gnss.v = q * Eigen::Vector3d(gps[3], gps[4], gps[5]);
        gnss_buff.push_back(gnss);
    }
}

//同步GPS 和IMU 数据 ,   eg 因为 imu 和 GPS的频率不一样所以需要数据同步
bool SyncData(bool inited)
{
    /*以gnss时间为准*/
    if (gnss_buff.empty())
    {
        return false;
    }
    current_gnss = gnss_buff.front();   //读取当前gnss队列第一个数据
    double sync_time = current_gnss.time;

/*sync groundtruth 数据*/
//gt时间同步
    while (gt_buff.size() > 1)
    {
        if(gt_buff[1].time < sync_time)
        {
            gt_buff.pop_front();       //将不对齐的时间删除
        }else
        {
            break;
        }
    }
//gt数据同步
if(gt_buff.size() > 1)
{   
    PoseData front_data = gt_buff.at(0);    //上一帧数据
    PoseData back_data = gt_buff.at(1);
    double front_scale = (back_data.time - sync_time) / (back_data.time - front_data.time); 
    double back_scale = (sync_time -front_data.time) /  (back_data.time - front_data.time);
    current_gt.time = sync_time ;
    //插值
    current_gt.state.p = front_data.state.p  * front_scale  + back_data.state.p *  back_scale;
    current_gt.state.v = front_data.state.v   * front_scale  + back_data.state.v *  back_scale;
    current_gt.state.q= front_data.state.q.slerp(front_scale,back_data.state.q);
    current_gt.state.bg = front_data.state.bg *  front_scale  + back_data.state.bg * back_scale;
    current_gt.state.ba = front_data.state.ba * front_scale   + back_data.state.ba * back_scale;
}else{
    return false;
}

/*sync imu 数据*/
// imu  时间同步
    while (!inited && imu_buff.size() > 1)
    {
        if (imu_buff[1].time < sync_time)
        {
            imu_buff.pop_front();
        }
        else
        {
            break;
        }
    }

// imu 数据同步
if (imu_buff.size() > 1)
{
    /*IMU 未初始化的数据同步*/
    if (!inited)
    {
        current_imu.clear();
        IMUData front_data = imu_buff.at(0);
        IMUData back_data = imu_buff.at(1);
        IMUData synced_data;

        double front_scale = (back_data.time - sync_time) / (back_data.time - front_data.time);
        double back_scale = (sync_time - front_data.time) / (back_data.time - front_data.time);
        synced_data.time = sync_time;
        synced_data.acc = front_data.acc * front_scale + back_data.acc * back_scale;
        synced_data.gyro = front_data.gyro * front_scale + back_data.gyro * back_scale;
        current_imu.push_back(synced_data);
        imu_buff.pop_front();
        gnss_buff.pop_front();
        // std::cout << std::setprecision(12) << "sync_time " << sync_time
        //           << " current_imu.time " << current_imu.front().time
        //           << "  " << current_imu.back().time << std::endl;
        return true;
    }

    if (imu_buff.back().time < sync_time)
    {
        return false;
    }

        while (current_imu.size() > 1)
    {
        current_imu.pop_front();
    }

    while (imu_buff.front().time < sync_time)
    {
        IMUData temp = imu_buff.front();
        imu_buff.pop_front();
        current_imu.push_back(temp);     // 将新的IMU数据放在deque队列末尾
    }
    IMUData front_data = current_imu.back();
    IMUData back_data = imu_buff.at(0);      //取 刚好比 sync_time 大的imu 数据
    IMUData synced_data;
    //插值
    double front_scale = (back_data.time - sync_time) / (back_data.time - front_data.time);
    double back_scale = (sync_time - front_data.time) / (back_data.time - front_data.time);
    synced_data.time = sync_time;
    synced_data.acc = front_data.acc * front_scale  +  back_data.acc  * back_scale; 
    synced_data.gyro = front_data.gyro * front_scale  + back_data.gyro * back_scale;
    current_imu.push_back(synced_data);

    gnss_buff.pop_front();
    return true;
}else{
    return false;
    }
}

bool InitSensor()          //初始化 传感器
{
    while (!gnss_buff.empty())
    {
         if (imu_buff.front().time >  gnss_buff.front().time)
         {
              gnss_buff.pop_front();       //删除时间不匹配的 gnss 数据,保证 imu的数据在前
         }else
         {
             return true;
         }
    }
    return  false;
}

bool InitPose()
{
    static bool pose_inited = false;
    if (pose_inited)
    {
        return true;
    }
    if (!SyncData(false))
    {
        return false;
    }
    current_pose.time = current_gt.time;
    current_pose.state.p = current_gt.state.p;
    current_pose.state.q = current_gt.state.q;
    current_pose.state.v = current_gt.state.v;
    current_pose.state.bg = current_gt.state.bg;
    current_pose.state.ba = current_gt.state.ba;
    current_error_state.x.setZero();
    current_error_state.p.setZero();
    current_error_state.p.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity() * init_noise[0];
    current_error_state.p.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity() * init_noise[1];
    current_error_state.p.block<3, 3>(6, 6) = Eigen::Matrix3d::Identity() * init_noise[2];
    current_error_state.p.block<3, 3>(9, 9) = Eigen::Matrix3d::Identity() * init_noise[3];
    current_error_state.p.block<3, 3>(12, 12) = Eigen::Matrix3d::Identity() * init_noise[4];
    pose_inited = true;
    return true;
}

 
bool Predict()  // 预测
{
    current_pose.time  = current_gt.time;
    Eigen::Vector3d  pp =  current_pose.state.p;
    Eigen::Vector3d  vv  =  current_pose.state.v;
    Eigen::Quaterniond qq = current_pose.state.q;
    double w = 7.27220521664304e-05;    // 地球自转速度
    Eigen::Vector3d gn(0, 0, -9.79484197226504);    // 重力加速度
    Eigen::Vector3d w_ie_n(0, w * std::cos(current_gnss.lla[0] * M_PI / 180),
                           w * std::sin(current_gnss.lla[0] * M_PI / 180));
    double rm = 6353346.18315;       // 短半轴
    double rn = 6384140.52699;        // 长半轴
    Eigen::Vector3d w_en_n(-vv[1] / (rm + current_gnss.lla[2]),
                           vv[0] / (rn + current_gnss.lla[2]),
                           vv[0] / (rn + current_gnss.lla[2]) * std::tan(current_gnss.lla[0] * M_PI / 180));
    Eigen::Vector3d w_in_n = w_ie_n + w_en_n;
    for (int i = 1; i < current_imu.size(); ++i)
    {
        double dt = current_imu[i].time - current_imu[i - 1].time;
        Eigen::Vector3d wtemp = w_in_n * dt;
        double angle = wtemp.norm();
        Eigen::Quaterniond qn(1, 0, 0, 0);
        if (angle != 0)
        {
            wtemp = wtemp / angle;
            wtemp = std::sin(angle / 2) * wtemp;
            qn = Eigen::Quaterniond(std::cos(angle / 2), wtemp[0], wtemp[1], wtemp[2]);
        }
        qn.normalize();   // 地球自转的角增量

    Eigen::Vector3d wb = 0.5* current_imu[i - 1].gyro  + 0.5*current_imu[i].gyro;
    wb = wb + current_pose.state.bg;
    wb = wb * dt;
    angle = wb.norm();
    Eigen::Quaterniond qb(1,0,0,0);
    if (angle != 0)
    {
        wb = wb / angle;
        wb = std::sin(angle / 2) * wb;
        qb = Eigen::Quaterniond(std::cos(angle / 2),wb[0],wb[1],wb[2] );
    }
    qb.normalize();     //载体角增量

    Eigen::Quaterniond  qq2 = qn.inverse() * qq * qb;   // 下一时刻的旋转矩阵
    Eigen::Vector3d f1 = current_imu[i-1].acc;
    f1 = f1 + current_pose.state.ba;
    Eigen::Vector3d f2 = current_imu[i].acc;
    f2 = f2 + current_pose.state.ba;
    Eigen::Vector3d vv2 = vv + dt * (0.5* (qq * f1 + qq * f2) + gn);  //下一时刻的速度
    Eigen::Vector3d pp2 = pp + 0.5*dt* (vv + vv2);
    pp = pp2;
    vv  = vv2;
    qq = qq2;
}   
current_pose.state.p = pp;    //更新当前位置
current_pose.state.v =  vv;
current_pose.state.q = qq;

Ft = Eigen::Matrix<double,15,15>::Zero();    // 状态转移矩阵
Ft.block<3,3>(0,3) = Eigen::Matrix<double, 3 ,3>::Identity();
Eigen::Matrix<double,3,3>temp = Eigen::Matrix<double,3,3>::Zero();
Eigen::Vector3d ff = current_imu.back().acc;
// F23
ff = qq*ff;
temp(0,1) = -ff[2];
temp(0,2) = ff[1];
temp(1,0) = ff[2];
temp(1, 2) = -ff[0];
temp(2, 0) = -ff[1];
temp(2, 1) = ff[0];
Ft.block<3,3>(3,6) = temp;
// Cb_n
Ft.block<3,3>(3,12) = qq.toRotationMatrix();
temp.setZero();
// F33 
temp(0, 1) = w_ie_n(2);
temp(0, 2) = -w_ie_n(1);
temp(1, 0) = -w_ie_n(2);
temp(2, 0) = w_ie_n(1);
Ft.block<3,3>(6,6)  = temp;
//- Cb_n
Ft.block<3,3>(6,9) =  -Ft.block<3,3>(3,12);
Eigen::Matrix<double,15,6>  Bt = Eigen::Matrix<double,15,6>::Zero();
Bt.block<3,3>(3,3) = Ft.block<3,3>(3,12);
Bt.block<3,3>(6,0) = Ft.block<3, 3>(6, 9);
T = current_imu.back().time  - current_imu.front().time; 
//   Qso 离散滤波器
Ft = Eigen::Matrix<double,15 ,15>::Identity() + Ft * T ;      // Fk-1 ,上一时刻的状态转移矩阵
Bt = Bt * T ;                                                                                           // Bk-1 ,上一时刻的观测矩阵
Eigen::Matrix<double,6,1> W = Eigen::Matrix<double,6,1>::Zero();    //器件噪声 ,一般指 IMU的零偏不稳定系
// eskf 先验
current_error_state.x   = Ft * current_error_state.x  + Bt * W;   
Eigen::Matrix<double, 6, 6> Q = Eigen::Matrix<double, 6, 6>::Identity();                //  惯性噪声
Q.block<3,3>(0,0) = Eigen::Matrix<double,3,3>:: Identity()* gyro_noise * gyro_noise;
Q.block<3,3>(3,3) = Eigen::Matrix<double,3,3>:: Identity()* acc_noise  * acc_noise;
current_error_state.p = Ft * current_error_state.p * Ft.transpose() + Bt * Q * Bt.transpose();
return  true ;
}


// 修正融合 
bool Correct()
{
    double geo_x, geo_y, geo_z;
    geo_converter.Forward(current_gnss.lla(0), current_gnss.lla(1),
                          current_gnss.lla(2), geo_x, geo_y, geo_z);      // 将经纬度处理为  东北天
    Eigen::Vector3d gnss_xyz(geo_x, geo_y, geo_z);

    Y.block<3,1>(0,0) = current_pose.state.p -  gnss_xyz;  // 观测误差

    Gt = Eigen::Matrix<double,3,15>::Zero();
    Gt.block<3,3>(0,0) = Eigen::Matrix<double,3,3>::Identity();
    Eigen::Matrix<double,3,3> Ct = Eigen::Matrix<double,3,3>::Identity();

    Eigen::Matrix<double, 3, 3> R = Eigen::Matrix<double, 3, 3>::Identity();    // 观测噪声, gps噪声
    R.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity() * dp_noise * dp_noise;
    Eigen::Matrix<double, 15, 3> K = current_error_state.p * Gt.transpose() * (Gt * current_error_state.p * Gt.transpose() + Ct * R * Ct.transpose()).inverse();  // kf 增益

    // 计算后验
    current_error_state.p = (Eigen::Matrix<double, 15, 15>::Identity() - K * Gt) * current_error_state.p;     // 后验方差
    current_error_state.x = current_error_state.x + K * (Y - Gt * current_error_state.x);                                      // 后验状态
    // 位姿、姿态 update 
    current_pose.state.p = current_pose.state.p - current_error_state.x.block<3,1>(0,0);
    current_pose.state.v = current_pose.state.v - current_error_state.x.block<3,1>(3,0);
         Eigen::Vector3d dphi_dir = current_error_state.x.block<3, 1>(6, 0);
    double dphi_norm = dphi_dir.norm();
    if (dphi_norm != 0)
    {
        dphi_dir = dphi_dir / dphi_norm;
        dphi_dir = dphi_dir * std::sin(dphi_norm / 2);
    }
    Eigen::Quaterniond temp2(std::cos(dphi_norm / 2), dphi_dir[0], dphi_dir[1], dphi_dir[2]);
    current_pose.state.q = temp2 * current_pose.state.q;     // 更新旋转矢量
    current_pose.state.q.normalize();
    current_pose.state.bg = current_pose.state.bg - current_error_state.x.block<3, 1>(9, 0);
    current_pose.state.ba = current_pose.state.ba - current_error_state.x.block<3, 1>(12, 0);
    current_error_state.x.setZero();        // 清空误差
    return true;
}


/*保存观测度分析所需的F G 和 Y*/
bool SaveFG()    
{
    if (FGs.size() > FGsize)
    {
        return true;
    }
    if (FGs.empty())
    {
        FG fg;
        fg.time = current_gt.time;
        // fg.F = Ft;
        fg.F = Ft - Eigen::Matrix<double, 15, 15>::Identity();
        // fg.F = (Ft - Eigen::Matrix<double, 15, 15>::Identity()) / T;
        fg.G = Gt;
        fg.Y.push_back(Y);
        FGs.push_back(fg);
    }
    else
    {
        if (FGs.back().Y.size() == 15)
        {
            if (current_gt.time - FGs.back().time < time_interval || FGs.size() >= FGsize)
            {
                return true;
            }
            FG fg;
            fg.time = current_gt.time;
            // fg.F = Ft;
            fg.F = Ft - Eigen::Matrix<double, 15, 15>::Identity();
            // fg.F = (Ft - Eigen::Matrix<double, 15, 15>::Identity()) / T;
            fg.G = Gt;
            fg.Y.push_back(Y);
            FGs.push_back(fg);
        }
        else
        {
            FGs.back().Y.push_back(Y);
        }
        
    }
    return true;
}


/*滤波*/
bool Filter()
{
    Predict();
    if (correct)
    {
        Correct();
    }
    return true;
}

void SavePose(std::ofstream &save_points, PoseData &pose)
{
    Eigen::Quaterniond qtemp = pose.state.q;
    // if (qtemp.w() < 0)
    // {
    //     qtemp.coeffs() = -1.0 * qtemp.coeffs();
    // }
    double angle = std::acos(qtemp.w()) * 2;
    double sin_angle = std::sin(angle / 2);
    Eigen::Vector3d dir(0, 0, 0);
    if (sin_angle != 0)
    {
        dir(0) = qtemp.x() / sin_angle;
        dir(1) = qtemp.y() / sin_angle;
        dir(2) = qtemp.z() / sin_angle;
        dir = dir * angle;
    }
    save_points.precision(12);
    save_points << pose.time
                << "," << pose.state.p(0)
                << "," << pose.state.p(1)
                << "," << pose.state.p(2)
                << "," << pose.state.v(0)
                << "," << pose.state.v(1)
                << "," << pose.state.v(2)
                // << "," << pose.state.q.x()
                // << "," << pose.state.q.y()
                // << "," << pose.state.q.z()
                // << "," << pose.state.q.w()
                << "," << dir(0)
                << "," << dir(1)
                << "," << dir(2)
                << "," << pose.state.bg(0)
                << "," << pose.state.bg(1)
                << "," << pose.state.bg(2)
                << "," << pose.state.ba(0)
                << "," << pose.state.ba(1)
                << "," << pose.state.ba(2)
                << std::endl;
}
bool SaveData()    // 保存数据
{
    SavePose(gt_ofs, current_gt);
    SavePose(pose_ofs, current_pose);
}

int main(int argc, char const *argv[])
{
    std::vector<std::string> path;

    path.push_back("../../gnss-ins-sim/imu_data/data7/time.csv");
    path.push_back("../../gnss-ins-sim/imu_data/data7/accel-0.csv");
    path.push_back("../../gnss-ins-sim/imu_data/data7/gyro-0.csv");
    path.push_back("../../gnss-ins-sim/imu_data/data7/gps_time.csv");
    path.push_back("../../gnss-ins-sim/imu_data/data7/gps-0.csv");
    path.push_back("../../gnss-ins-sim/imu_data/data7/ref_pos.csv");
    path.push_back("../../gnss-ins-sim/imu_data/data7/ref_vel.csv");
    path.push_back("../../gnss-ins-sim/imu_data/data7/ref_att_quat.csv");
    ReadData(path);
    gt_ofs.open("../../gnss-ins-sim/imu_data/data7/gt.txt", std::fstream::out);
    pose_ofs.open("../../gnss-ins-sim/imu_data/data7/pose.txt", std::fstream::out);
    sv_ofs.open("../../gnss-ins-sim/imu_data/data7/sv.txt", std::fstream::out);
    FGs.clear();

    YAML::Node yaml_node = YAML::LoadFile("../param.yaml");
    gyro_noise = yaml_node["gyro_noise"].as<double>();
    acc_noise = yaml_node["acc_noise"].as<double>();
    dp_noise = yaml_node["dp_noise"].as<double>();
    init_noise = yaml_node["init_noise"].as<std::vector<double>>();
    FGsize = yaml_node["FGsize"].as<int>();
    end_time = yaml_node["end_time"].as<double>();
    time_interval = yaml_node["time_interval"].as<double>();
    correct = yaml_node["correct"].as<bool>();


    if (!InitSensor())
    {
        std::cerr << "InitSensor Error!!!" << std::endl;
        return -1;
    }
    if (!InitPose())
    {
        std::cerr << "InitPose Error!!!" << std::endl;
        return -1;
    }
    SaveData();
    while (SyncData(true))
    {
        Filter();
        SaveData();
        SaveFG();
        if (current_gt.time > end_time)
        {
            break;
        }
    }

    Eigen::MatrixXd  Qso(3*15*FGs.size(),15);
    Eigen::MatrixXd  Ys(3*15*FGs.size(),1);
    for (int i = 0; i < FGs.size(); ++i)
    {
        Eigen::Matrix<double,15,15> Fn = Eigen::Matrix<double,15,15>::Identity();
        for (int j = 0; j < 15; j++)
        {
            if (j > 0)
            {
                Fn = Fn * FGs[i].F;
            }
            Ys.block<3, 1>(i*3*15+3*j, 0) = FGs[i].Y[j];
            Qso.block<3, 15>(i*3*15+3*j, 0) = FGs[i].G * Fn ;    // 构建可观测性矩阵
        }
    }

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(Qso, Eigen::ComputeFullU | Eigen::ComputeFullV);
    std::cout << Qso.rows() << ", " << Qso.cols() << std::endl;
    // std::cout << svd.singularValues() << std::endl;
    for (int i = 0; i < 15; ++i)
    {
        double temp = (svd.matrixU().row(i) * Ys)[0] / svd.singularValues()[i];
        Eigen::MatrixXd Xi = temp * svd.matrixV().col(i);
        // std::cout << Xi.transpose() << std::endl;
        Eigen::MatrixXd::Index maxRow, maxCol;
        Xi = Xi.cwiseAbs();   //  Xi的绝对值
        double maxvalue = Xi.maxCoeff(&maxRow, &maxCol);
     std::cout << svd.singularValues()(i) / svd.singularValues()(0) << "," << maxRow << std::endl;
        sv_ofs << svd.singularValues()(i) / svd.singularValues()(0) << "," << maxRow << std::endl;
    }

    return 0;
}