多传感器融合定位（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 zEigen::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_errimu_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 datagps_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-gyroEigen::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;
}