武汉大学KF-GINS开源代码学习2--gnssUpdate

人生导航CLK

于 2025-03-03 17:18:56 发布

阅读量1.4k

点赞数 40

分类专栏：组合导航 GINS 文章标签：算法

本文链接：https://blog.youkuaiyun.com/clk88888/article/details/145888093

版权

组合导航同时被 2 个专栏收录

3 篇文章

订阅专栏

GINS

3 篇文章

订阅专栏

这次进行gnssUpdate的学习，这段代码是 GIEngine 类中的 gnssUpdate 方法，其主要功能是利用 GNSS（全球导航卫星系统）数据对系统状态进行更新，采用的是扩展卡尔曼滤波（EKF）算法。

EKF更新

首先由EKF量测更新的公式可知，其输入需要量测信息，量测矩阵H和噪声协方差矩阵R，因此代码如下所示：

    EKFUpdate(dz, H_gnsspos, R_gnsspos);

${{\bf{K}}_k} = {{\bf{P}}_{k/k - 1}}{\bf{H}}_k^{\rm{T}}{({{\bf{H}}_k}{{\bf{P}}_{k/k - 1}}{\bf{H}}\displaystyle _k^{\rm{T}} + {{\bf{R}}_k})^{ - 1}}$

因为H中有变量，见如下的公式xxx，所以需要将H作为函数变量，而不是类GEigine中的成员函数，而协方差矩阵Cov_

为什么与滤波有关的只将以下三个元素定义为类GEigine中的成员变量呢？

    // Kalman滤波相关
    // ekf variables
    Eigen::MatrixXd Cov_;
    Eigen::MatrixXd Qc_;
    Eigen::MatrixXd dx_;

笔者认为可能是因为这三元素在类GIEngine多个成员函数中都要用到，为了方便起见，就将其定义为成员变量，方便调用。

公式如下所示：

${{\bf{P}}_k} = ({\bf{I}} - {\bf{K}}_k^{}{\bf{H}}_k^{}){{\bf{P}}_{k/k - 1}}{({\bf{I}} - {\bf{K}}_k^{}{\bf{H}}_k^{})^{\rm{T}}} + {\bf{K}}_k^{}{{\bf{R}}_k}{\bf{K}}_k^{\rm{T}}$

${{\bf{\hat X}}_k} = {{\bf{\hat X}}_{k/k - 1}} + {{\bf{K}}_k}({{\bf{Z}}_k} - {{\bf{H}}_k}{{\bf{\hat X}}_{k/k - 1}})$

对应的代码如下所示：

    auto temp         = H * Cov_ * H.transpose() + R;
    Eigen::MatrixXd K = Cov_ * H.transpose() * temp.inverse();

    Eigen::MatrixXd I;
    I.resizeLike(Cov_);
    I.setIdentity();
    I = I - K * H;

    dx_  = dx_ + K * (dz - H * dx_);
    Cov_ = I * Cov_ * I.transpose() + K * R * K.transpose();  //转置

前面加上一些矩阵维数的判断。

    assert(H.cols() == Cov_.rows());
    assert(dz.rows() == H.rows());
    assert(dz.rows() == R.rows());
    assert(dz.cols() == 1);

新息dz

在卡尔曼滤波等状态估计方法中，新息是指实际测量值与基于系统模型预测的测量值之间的差值。对于 GNSS 位置测量而言，GNSS 位置测量新息就是 GNSS 接收机实际测得的位置信息（如经度、纬度、高度）与根据之前的状态估计和系统模型预测得到的当前位置信息之间的差异。

（1）将IMU 位置转到 GNSS 天线相位中心位置：由于惯导的几何中心和GNSS的几何中心不在一起，因此需要进行转换，在武汉大学的代码中，是将惯导的输出转换到GNSS的几何位置处，即antenna_pos，求解公式如下所示：

${{\bf{p}}_{{\rm{GNSS}}}} = {{\bf{p}}_{{\rm{INS}}}} + {{\bf{M}}_{pv}}{\bf{C}}_b^n{{\bf{l}}^b}$

其中Mpv是将n系相对位置转大地坐标相对位置，公式如下所示：

${{\bf{M}}_{pv}} = \left[ {\begin{array}{*{20}{c}} 0&{ 1/{R_{Mh}}}&0\\ { \sec L/{R_{Nh}}}&0&0\\ 0&0&{ - 1} \end{array}} \right]$

Mpv的推导是根据位置更新公式 $\dot L = \frac{1}{{{R_M} + h}}{v_{\rm{N}}},\begin{array}{*{20}{c}} {}&{} \end{array}\dot \lambda = \frac{{\sec L}}{{{R_N} + h}}{v_{\rm{E}}},\begin{array}{*{20}{c}} {}&{} \end{array}\dot h = {-v_{\rm{D}}}$ 来的，注意不同的导航坐标系，Mpv的公式是不一样的，记得修改。

（2）将得到的antenna_pos减去GNSS的位置，这是n系下的相对位置，由于定义的状态变量中的位置误差是大地坐标，即纬度、经度和高程，因此需要进行转换，代码如下所示：

    Eigen::MatrixXd dz;
    dz = Dr * (antenna_pos - gnssdata.blh);

最终得到新息dz。

量测矩阵H_gnsspos

H的列数由状态变量的维数决定，行数由量测的维数决定，H里面的值由状态变量的定义有关，也和提供的量测信息有关，本代码中GNSS只提供位置信息，因此H是3行。

推导公式见讲义：

由本代码可知，状态变量为

    enum StateID { P_ID = 0, V_ID = 3, PHI_ID = 6, BG_ID = 9, BA_ID = 12, SG_ID = 15, SA_ID = 18 };

因此，将值写入对应的位置即可，代码如下所示：

    Eigen::MatrixXd H_gnsspos;
    H_gnsspos.resize(3, Cov_.rows());
    H_gnsspos.setZero();
    H_gnsspos.block(0, P_ID, 3, 3)   = Eigen::Matrix3d::Identity();
    H_gnsspos.block(0, PHI_ID, 3, 3) = Rotation::skewSymmetric(pvacur_.att.cbn * options_.antlever);

观测噪声协方差矩阵R

    Eigen::MatrixXd R_gnsspos;
    R_gnsspos = gnssdata.std.cwiseProduct(gnssdata.std).asDiagonal();

cwiseProduct(gnssdata.std)：cwiseProduct 是 Eigen 库中的一个成员函数，用于进行逐元素相乘操作。这里将 gnssdata.std 向量与自身逐元素相乘，得到一个新的向量，新向量的每个元素是原向量对应元素的平方。
asDiagonal()：asDiagonal 是 Eigen 库中向量的一个成员函数，它将向量转换为一个对角矩阵，向量的元素依次作为对角矩阵的对角元素，非对角元素都为 0。

GNSS是一个结构体，定义了一个结构体对象gnss，并调用里面的成员变量std去初始化R_gnsspos，由于R_gnsspos是观测噪声协方差矩阵，所以要将每一个元素取平方从标准差变为方差。将R定义为Eigen::MatrixXd是因为其由观测量的数量来决定，有可能只有位置量测，也有可能有速度和位置作为量测。

GNSS标志位

    gnssdata.isvalid = false;

将 gnssdata 的 isvalid 标志设置为 false，表示该 GNSS 数据已经被使用，后续不再使用该数据进行更新。

常见的库函数

定义一个和其他矩阵维数相同的矩阵

matlab一行就能就解决，C++需要三行，学习如何定义。

I = eye(size(B)); //MATLAB

    Eigen::MatrixXd I;
    I.resizeLike(Cov_);
    I.setIdentity();

resize：直接指定新的行数和列数来调整对象的大小。
resizelike：将对象的大小调整为另一个参考对象的大小，更方便在需要根据已有对象的尺寸进行调整时使用。

transpose()

矩阵转置

inverse()

矩阵求逆