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

本次作业摘自 张松鹏大哥的优秀作业

代码下载 /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 为修改后适应matlaba的可视化代码，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;// F23ff = 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_nFt.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_nFt.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 // Degree2Radiusstruct 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;// F23ff = 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_nFt.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_nFt.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;}

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