本着“干中学”的原则，学习了武汉大学的KF-GINS代码，一边学习代码编程的思路，一边学习C++的知识点和一些常见库文件中的函数，编程的思路应该是从后往前，编写代码是从前往后。首先从insPropagation开始写起吧，前面的内容后续再补充了。
目录
insPropagation
进行INS状态更新(IMU机械编排算法), 并计算IMU状态转移矩阵和噪声阵
void insPropagation(IMU &imupre, IMU &imucur);其中输入值是前一时刻(k-1)IMU的值和当前时刻(k)IMU的值，IMU是一个结构体，所以在types.h中定义，但是不知为什么把里程计的速度也放入IMU类中？
IMU imupre_;
IMU imucur_;typedef struct IMU {
double time;
double dt;
Vector3d dtheta; //角速度增量
Vector3d dvel; //线速度增量
double odovel; //里程计（odometer）测量的速度，标量
} IMU;1.IMU数据误差补偿
imuCompensate(imucur);imuCompensate代码如下所示：
imu.dtheta -= imuerror_.gyrbias * imu.dt;
imu.dvel -= imuerror_.accbias * imu.dt;
Eigen::Vector3d gyrscale, accscale;
gyrscale = Eigen::Vector3d::Ones() + imuerror_.gyrscale;
accscale = Eigen::Vector3d::Ones() + imuerror_.accscale;
imu.dtheta = imu.dtheta.cwiseProduct(gyrscale.cwiseInverse());
imu.dvel = imu.dvel.cwiseProduct(accscale.cwiseInverse());主要是用陀螺零偏和标度因数去校准IMU的输出值，IMU的输出值imu已定义，误差值imuerror_也在gi_engine.h中定义了，如下所示：
ImuError imuerror_;//是gi_engine.h的私有成员变量对于零偏而言，直接用陀螺和加速度计测量值减去对应零偏即可；
对于标度因数而言，标度误差是指IMU传感器的测量值与真实值之间的比例偏差。例如，如果陀螺仪的标度误差为[0.01, 0.02, 0.03]，则实际测量值会比真实值分别放大1.01、1.02和1.03倍。为了校正这种误差，需要将测量值除以标度因子（即取倒数后相乘）。
同时要注意到此处的imuerror_是会变化的，随着量测更新，会得到新估计的误差值，对误差值imuerror_进行校准(再有下一时刻IMU数据来临时，进行惯导更新算法前，即可采用最新估计的误差值对IMU进行误差补偿)，如下所示：
vectemp = dx_.block(BG_ID, 0, 3, 1);
imuerror_.gyrbias += vectemp;
vectemp = dx_.block(BA_ID, 0, 3, 1);
imuerror_.accbias += vectemp;
vectemp = dx_.block(SG_ID, 0, 3, 1);
imuerror_.gyrscale += vectemp;
vectemp = dx_.block(SA_ID, 0, 3, 1);
imuerror_.accscale += vectemp;注意：此处是标度因数的误差量δK，而不是标度因数K。
2.IMU状态更新(惯导更新算法)
INSMech::insMech(pvapre_, pvacur_, imupre, imucur);函数内容如下所示：
// 依次进行速度更新、位置更新、姿态更新, 不可调换顺序
velUpdate(pvapre, pvacur, imupre, imucur);
posUpdate(pvapre, pvacur, imupre, imucur);
attUpdate(pvapre, pvacur, imupre, imucur);下一篇博客详细介绍。
3.EKFPredict(Phi, Qd)进行状态预测
从EKF的预测公式编写对应的代码，代码如下所示：
void GIEngine::EKFPredict(Eigen::MatrixXd &Phi, Eigen::MatrixXd &Qd) {
assert(Phi.rows() == Cov_.rows());
assert(Qd.rows() == Cov_.rows());
// 传播系统协方差和误差状态
// propagate system covariance and error state
Cov_ = Phi * Cov_ * Phi.transpose() + Qd;
dx_ = Phi * dx_;
}其中 dx_和Cov_是组合导航中需要用到的变量，因此将其定义到GIEngine类中：
Eigen::MatrixXd Cov_;
Eigen::MatrixXd Qc_;
Eigen::MatrixXd dx_;
从代码可以看出需要计算得到Phi和Qd，才能进行EKF预测更新，下面就介绍计算的方法：
计算Phi
（1）Phi是离散化之后的值，离散化的公式如下所示：
Phi.setIdentity();
Phi = Phi + F * imucur.dt; //离散化（2）确定F的思路如下：
①首先要确定状态变量是什么，在GIEngine.h之中定义了枚举类型，看以看出状态变量的类型：
enum StateID { P_ID = 0, V_ID = 3, PHI_ID = 6, BG_ID = 9, BA_ID = 12, SG_ID = 15, SA_ID = 18 };
具体的公式可见《惯性导航原理与组合导航课程讲义》：
（1）位置误差的导数关于位置误差和速度误差的关系：
对应的代码如下所示：
// position error
temp.setZero();
temp(0, 0) = -pvapre_.vel[2] / rmh;
temp(0, 2) = pvapre_.vel[0] / rmh;
temp(1, 0) = pvapre_.vel[1] * tan(pvapre_.pos[0]) / rnh;
temp(1, 1) = -(pvapre_.vel[2] + pvapre_.vel[0] * tan(pvapre_.pos[0])) / rnh;
temp(1, 2) = pvapre_.vel[1] / rnh;
F.block(P_ID, P_ID, 3, 3) = temp;
F.block(P_ID, V_ID, 3, 3) = Eigen::Matrix3d::Identity();（2）速度误差的导数关于速度误差、失准角误差、陀螺零偏和加表零偏之间的关系：
对应的代码如下所示：
temp.setZero();
temp(0, 0) = -2 * pvapre_.vel[1] * WGS84_WIE * cos(pvapre_.pos[0]) / rmh -
pow(pvapre_.vel[1], 2) / rmh / rnh / pow(cos(pvapre_.pos[0]), 2);
temp(0, 2) = pvapre_.vel[0] * pvapre_.vel[2] / rmh / rmh - pow(pvapre_.vel[1], 2) * tan(pvapre_.pos[0]) / rnh / rnh;
temp(1, 0) = 2 * WGS84_WIE * (pvapre_.vel[0] * cos(pvapre_.pos[0]) - pvapre_.vel[2] * sin(pvapre_.pos[0])) / rmh +
pvapre_.vel[0] * pvapre_.vel[1] / rmh / rnh / pow(cos(pvapre_.pos[0]), 2);
temp(1, 2) = (pvapre_.vel[1] * pvapre_.vel[2] + pvapre_.vel[0] * pvapre_.vel[1] * tan(pvapre_.pos[0])) / rnh / rnh;
temp(2, 0) = 2 * WGS84_WIE * pvapre_.vel[1] * sin(pvapre_.pos[0]) / rmh;
temp(2, 2) = -pow(pvapre_.vel[1], 2) / rnh / rnh - pow(pvapre_.vel[0], 2) / rmh / rmh +
2 * gravity / (sqrt(rmrn[0] * rmrn[1]) + pvapre_.pos[2]);
F.block(V_ID, P_ID, 3, 3) = temp;
temp.setZero();
temp(0, 0) = pvapre_.vel[2] / rmh;
temp(0, 1) = -2 * (WGS84_WIE * sin(pvapre_.pos[0]) + pvapre_.vel[1] * tan(pvapre_.pos[0]) / rnh);
temp(0, 2) = pvapre_.vel[0] / rmh;
temp(1, 0) = 2 * WGS84_WIE * sin(pvapre_.pos[0]) + pvapre_.vel[1] * tan(pvapre_.pos[0]) / rnh;
temp(1, 1) = (pvapre_.vel[2] + pvapre_.vel[0] * tan(pvapre_.pos[0])) / rnh;
temp(1, 2) = 2 * WGS84_WIE * cos(pvapre_.pos[0]) + pvapre_.vel[1] / rnh;
temp(2, 0) = -2 * pvapre_.vel[0] / rmh;
temp(2, 1) = -2 * (WGS84_WIE * cos(pvapre_.pos(0)) + pvapre_.vel[1] / rnh);
F.block(V_ID, V_ID, 3, 3) = temp;
F.block(V_ID, PHI_ID, 3, 3) = Rotation::skewSymmetric(pvapre_.att.cbn * accel);
F.block(V_ID, BA_ID, 3, 3) = pvapre_.att.cbn;
F.block(V_ID, SA_ID, 3, 3) = pvapre_.att.cbn * (accel.asDiagonal());
G.block(V_ID, VRW_ID, 3, 3) = pvapre_.att.cbn;失准角误差的导数关于速度误差、失准角误差、陀螺零偏和陀螺标度因数之间的关系：
对应的代码如下所示：
temp.setZero();
temp(0, 0) = -WGS84_WIE * sin(pvapre_.pos[0]) / rmh;
temp(0, 2) = pvapre_.vel[1] / rnh / rnh;
temp(1, 2) = -pvapre_.vel[0] / rmh / rmh;
temp(2, 0) = -WGS84_WIE * cos(pvapre_.pos[0]) / rmh - pvapre_.vel[1] / rmh / rnh / pow(cos(pvapre_.pos[0]), 2);
temp(2, 2) = -pvapre_.vel[1] * tan(pvapre_.pos[0]) / rnh / rnh;
F.block(PHI_ID, P_ID, 3, 3) = temp;
temp.setZero();
temp(0, 1) = 1 / rnh;
temp(1, 0) = -1 / rmh;
temp(2, 1) = -tan(pvapre_.pos[0]) / rnh;
F.block(PHI_ID, V_ID, 3, 3) = temp;
F.block(PHI_ID, PHI_ID, 3, 3) = -Rotation::skewSymmetric(wie_n + wen_n);
F.block(PHI_ID, BG_ID, 3, 3) = -pvapre_.att.cbn;
F.block(PHI_ID, SG_ID, 3, 3) = -pvapre_.att.cbn * (omega.asDiagonal());
G.block(PHI_ID, ARW_ID, 3, 3) = pvapre_.att.cbn;
//此处应该是负号才对？？误差的导数与误差之间的关系：
对应的代码如下所示：
F.block(BG_ID, BG_ID, 3, 3) = -1 / options_.imunoise.corr_time * Eigen::Matrix3d::Identity();
F.block(BA_ID, BA_ID, 3, 3) = -1 / options_.imunoise.corr_time * Eigen::Matrix3d::Identity();
F.block(SG_ID, SG_ID, 3, 3) = -1 / options_.imunoise.corr_time * Eigen::Matrix3d::Identity();
F.block(SA_ID, SA_ID, 3, 3) = -1 / options_.imunoise.corr_time * Eigen::Matrix3d::Identity();噪声矩阵的设置：
G.block(BG_ID, BGSTD_ID, 3, 3) = Eigen::Matrix3d::Identity();
G.block(BA_ID, BASTD_ID, 3, 3) = Eigen::Matrix3d::Identity();
G.block(SG_ID, SGSTD_ID, 3, 3) = Eigen::Matrix3d::Identity();
G.block(SA_ID, SASTD_ID, 3, 3) = Eigen::Matrix3d::Identity();注意：
此处的位置是e系下的，不是大地坐标（纬度 φ、经度 λ 和椭球高 h）
坐标系的定义不同，导致地球自转在导航系n的投影值不同，即wien不同，导致F矩阵的不同，记得区分。
计算Qd
从《惯性导航原理与组合导航课程讲义》编写公式：
// 计算系统传播噪声
// compute system propagation noise
Qd = G * Qc_ * G.transpose() * imucur.dt;
Qd = (Phi * Qd * Phi.transpose() + Qd) / 2;严恭敏老师《捷联惯导算法与组合导航原理》的书上是这样写的：
4. 用到的Eigen库中成员函数
resizeLike
Phi.resizeLike(Cov_);
F.resizeLike(Cov_);
Qd.resizeLike(Cov_);在 Eigen 库中，resizeLike 是一个用于调整矩阵或向量大小的成员函数，它会将目标矩阵或向量的大小调整为与另一个矩阵或向量的大小一致
setIdentity
在 Eigen 库中，setIdentity 是一个非常有用的成员函数，用于将矩阵或向量设置为单位矩阵（Identity Matrix）或单位向量。单位矩阵是一个方阵，其对角线上的元素为1，其余元素为0。
setIdentity 可以将一个矩阵或向量设置为单位矩阵或单位向量。它通常用于初始化矩阵或向量。
（1）对于矩阵，setIdentity 会将矩阵设置为单位矩阵。例如：
Eigen::MatrixXd A(3, 3); // 定义一个 3x3 的矩阵
A.setIdentity(); // 将矩阵 A 设置为单位矩阵运行后，矩阵 A 的内容为：
1 0 0
0 1 0
0 0 1（2）对于向量，setIdentity 会将向量的第1个元素设置为1，其余元素设置为0。例如：
Eigen::VectorXd v(4); // 定义一个 4 维向量
v.setIdentity(); // 将向量 v 设置为单位向量运行后，向量 v 的内容为：
1
0
0
0（3）setIdentity 还可以接受两个参数，分别指定行数和列数，用于创建特定大小的单位矩阵。例如：
Eigen::MatrixXd B;
B.setIdentity(4, 4); // 创建一个 4x4 的单位矩阵这等价于：
Eigen::MatrixXd B = Eigen::MatrixXd::Identity(4, 4);（4） 示例代码
以下是一个完整的示例，展示如何使用 setIdentity：
#include <iostream>
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;
int main() {
// 矩阵示例
MatrixXd A(3, 3);
A.setIdentity();
cout << "3x3 单位矩阵 A:\n" << A << endl;
// 向量示例
VectorXd v(4);
v.setIdentity();
cout << "4 维单位向量 v:\n" << v << endl;
// 参数化用法
MatrixXd B;
B.setIdentity(4, 4);
cout << "4x4 单位矩阵 B:\n" << B << endl;
return 0;
}运行结果：
3x3 单位矩阵 A:
1 0 0
0 1 0
0 0 1
4 维单位向量 v:
1
0
0
0
4x4 单位矩阵 B:
1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1注意事项
-
setIdentity是一个 成员函数，它会直接修改调用它的矩阵或向量的内容。 -
如果需要创建一个新的单位矩阵或向量，可以使用
Eigen::MatrixXd::Identity(rows, cols)或Eigen::VectorXd::Unit(size)。 -
setIdentity仅适用于 方阵（矩阵的行数和列数相等）或 向量（一维数组）。
setZero
-
setZero：用于将已存在的矩阵或向量的所有元素设置为零。 -
Zero：用于创建一个新的矩阵或向量，并初始化为零。
Eigen::Matrix3d mat;
mat << 1, 2, 3,
4, 5, 6,
7, 8, 9;
std::cout << "Original Matrix:\n" << mat << "\n\n";
// 使用 setZero 将矩阵所有元素设置为零
mat.setZero();
std::cout << "Matrix after setZero:\n" << mat << "\n\n";Eigen::VectorXd vec1 = Eigen::VectorXd::Zero(5); // 创建一个 5 维零向量
Eigen::MatrixXd mat1 = Eigen::MatrixXd::Zero(3, 4); // 创建一个 3x4 的零矩阵block
在 Eigen 库中，block 是一个非常强大的功能，用于操作矩阵或向量的子块（sub-block）。通过 block，你可以提取、修改或赋值矩阵的特定区域，而无需手动遍历元素。这使得代码更加简洁、高效且易于理解。
（1） block 的基本用法
block 的语法如下：
MatrixType::block<int>(int startRow, int startCol, int rows, int cols);-
startRow和startCol：子块的起始行和列索引（从0开始）。 -
rows和cols：子块的行数和列数。 -
返回值：返回一个子块的引用，可以用于读取或修改。
提取子块
Eigen::MatrixXd A = Eigen::MatrixXd::Random(4, 4); // 创建一个 4x4 的随机矩阵
Eigen::MatrixXd B = A.block<2, 2>(1, 1); // 提取从 (1,1) 开始的 2x2 子块修改子块
A.block<2, 2>(1, 1) = Eigen::MatrixXd::Identity(2, 2); // 将子块设置为单位矩阵（2）动态大小的子块
如果子块的大小是动态的（运行时确定），可以省略模板参数：
int rows = 2;
int cols = 2;
A.block(1, 1, rows, cols) = Eigen::MatrixXd::Zero(rows, cols); // 将子块设置为零矩阵（3）示例代码
以下是一个完整的示例，展示如何使用 block 来提取和修改矩阵的子块：
#include <iostream>
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;
int main() {
// 创建一个 4x4 的随机矩阵
MatrixXd A = MatrixXd::Random(4, 4);
cout << "原始矩阵 A:\n" << A << endl;
// 提取从 (1,1) 开始的 2x2 子块
MatrixXd B = A.block<2, 2>(1, 1);
cout << "提取的 2x2 子块 B:\n" << B << endl;
// 修改子块
A.block<2, 2>(1, 1) = MatrixXd::Identity(2, 2); // 将子块设置为单位矩阵
cout << "修改后的矩阵 A:\n" << A << endl;
// 提取第 2 行
VectorXd row = A.row(1);
cout << "第 2 行:\n" << row.transpose() << endl;
// 提取第 3 列
VectorXd col = A.col(2);
cout << "第 3 列:\n" << col << endl;
return 0;
}（4）输出结果
假设矩阵 A 是：
原始矩阵 A:
0.12 0.34 0.56 0.78
0.90 -0.12 0.34 -0.56
-0.78 0.90 -0.12 0.34
0.56 -0.78 0.90 -0.12运行结果：
提取的 2x2 子块 B:
-0.12 0.34
0.90 -0.12
修改后的矩阵 A:
0.12 0.34 0.56 0.78
0.90 1.00 0.00 -0.56
-0.78 0.00 1.00 0.34
0.56 -0.78 0.90 -0.12
第 2 行:
0.90 1.00 0.00 -0.56
第 3 列:
0.56
0.00
1.00
0.90
扫一扫
6771
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
