多传感器融合定位（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 五大重要公式

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

eskf 状态方程

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 认为不可观。

运行 plot_data.m 生成可视化分析
各状态量的收敛情况见下图，从图中可看出，航向角(8)，而这这个状态量对应的可观测度系数均非常小，为e-05量级，另外，z 轴角速度零偏的收敛速度很慢，直到 10分钟后才逐渐收敛，它对应的可观测度也很低，在 1.2e-7 左右，由于其仍然能够收敛，因此认为它是可观的。

通调整 FGsize，加大时变系统下的方程组数，可使变量可观

b) 匀速状态 -摘自 松鹏大哥作业
仿真命令参见附件文件 imu_def8.csv，b 系沿前向（y 轴正向）做匀速运动。各状态量的收敛情况见下图，可观测度系数见下表。从图中可看出，收敛情况与静止状态完全相同，航向角，x 轴加速度零偏和 y 轴加速度零偏均未收敛，这 3 个状态量对应的可观测度系数均很小，在 1e-33 到 1e-28 量级，因此 3 个状态量是不可观的。z 轴角速度零偏收敛速度很慢，对应的可观测度也很低，在 6e-8 左右，但仍然能够收敛，因此是可观的。


c) 加减速状态 -摘自 松鹏大哥作业
仿真命令参见附件文件 imu_def9.csv，b 系沿 x 轴和 y 轴两个方向分别做加减速运动，具体运动参考下图中的位置和速度变化曲线。可观测度系数见下表。各状态量对应的可观测度系数均在 7.9e-4 以上，而图中各状态量的误差值均在 0 值附近波动，可认为达到收敛状态。但由于 IMU 数据存在误差，各状态量均存在一定程度的误差。在加减速变换时，角度和零偏的误差值均有一定波动。可观测度系数中最小的 3 个是 z 轴角速度零偏，x 轴加速度零偏，y 轴加速度零偏，均在 1e-3 量级，3 个状态量观测度较低，而收敛
速度也较慢，特别是 2 个加速度水平零偏，在 20s 之后误差值才逐渐减小，因此对应的可观测度也最低。航向角的误差值在几次加减速后逐渐下降了一个数量级，对应的可观测度较前面 2 种情况也有大幅提高，约 0.012。

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

添加个人注解的 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;
}