0. 简介

最简作者在看PX4的相关内容，其中需要提取对yaw角的估计，所以针对性的对ECL EKF2中的EKF-GSF 偏航估计器进行学习。国内外相关的资料很少，这里主要基于《使用 IMU 和 GPS 进行偏航对准》这篇文章的内容，并结合PX4官网的内容，完成介绍。相关的代码在GIthub上，这里结合代码来阅读学习原作者的相关阐述。值得一提的是Github上有一位大佬提供了一套PX4 ECL EKF公式推导以及代码解析的内容

1. EKF-GSF 偏航估计器描述

该算法能在没有磁强计或外部偏航传感器的情况下运行，其目的是自动修正偏航误差引起的起飞后导航损失。EKF在内部运行一个附加的多假设滤波器，该滤波器使用状态为NE速度和偏航角的多个三态扩展卡尔曼滤波器（EKF）。然后使用高斯和滤波器（GSF）组合这些单独的偏航角估计。各个三态EKF使用IMU和GPS水平速度数据（加上可选的空速数据），并且不依赖于偏航角或磁力计测量的任何先验知识。

EKF-GSF 算法也是在 ECL EKF2 的 24 参数状态 EKF 之外又一个EKF实现，包括以下内容

• 使用互补滤波器的 5 组 AHRS 的解

  • 这些计算预测偏航角和向前、向右加速度。
  • 空速(测量或估计)用于固定翼飞行期间的向心加速度修正。

• 5 组 3 参数状态扩展卡尔曼滤波器

  • 状态为向北(N)、向东(E)速度和偏航角。
  • 偏航角估算开始时的角度间隔相等， $$\left[\begin{array}{ccccc} -4/5\pi & -2/5\pi & 0 & 2/5\pi & 4/5\pi\end{array}\right]$$ 。
  • GPS的向北(N)、向东(E)速度作为观测值。

• 高斯和滤波器(Gaussian Sum Filter)

  • 根据标准化 GPS 的向北(N)、向东(E)速度新息数值级别计算每个 EKF 的权重。总权重为 1 。
  • 输出偏航角 $$\psi$$ 估计值，这是单个 EKF 估计值的加权平均值。

GSF-EKF过程框图如下：

在我们拿到ulg文件后发现：

1. vehicle_angular_velocity大概45~50hz
2. vehicle_attitude大概20hz
   这就代表我们IMU的更新频率并不是很高，但是取得了比较好的收益

2. 偏航估计器的算法直觉

EKF-GSF 算法是一个很新奇也很有创意的算法。首先该算法一个假设就是偏航角由 xy 水平面的向北(N)、向东(E)的增量速度和绕 z 轴旋转的增量角度所驱动，所以偏航角的测量误差也和这 3 个值的测量噪声相关

\psi_{error}\propto\left(\sigma_{\Delta V_{x}}^{2},\sigma_{\Delta V_{y}}{2},\sigma_{\Delta\psi}{2}\right)

这 3 个值和 3 个噪声由 IMU 提供。于是我们可以在 xy 水平面上面预置 5 个偏航角 \psi_{i},i\in1\sim5 ，每个偏航角由这 3 个值和 3 个噪声驱动演化，独立进行时间更新。在测量更新阶段，我们观测 GPS 提供的 xy 水平面的向北(N)、向东(E)的速度，因为残差的存在，这 5 个偏航角 \psi_{i} 会向真实的偏航角 \psi_{true} 聚拢，但是真实的偏航角 \psi_{true} 不可见。

不过我们可以通过高斯和的算法，计算这 5 个偏航角\psi_{i}的权重并相加，得到距离真实的偏航角 \psi_{true} 最近的复合偏航角 \psi_{GSF} 。

因此该算法就可以在没有磁强计的情况下估计偏航角，并且是水平速度变化越大，收敛越快。

3. 偏航估计器的初始化以及对齐

3.1 偏航估计器的初始化

初始化的关键在于在 xy 平面上预制 5 个偏航角，平均间隔，平均权重。其它初始化都是常规操作。

void EKFGSF_yaw::initialiseEKFGSF()//初始化EKF GSF
{
    _gsf_yaw = 0.0f;//初始化偏航角
    _ekf_gsf_vel_fuse_started = false;//初始化速度融合标志位
    _gsf_yaw_variance = _m_pi2 * _m_pi2;//初始化偏航角方差
    _model_weights.setAll(1.0f / (float)N_MODELS_EKFGSF);  // 所有的过滤器模型都以相同的权重开始

    memset(&_ekf_gsf, 0, sizeof(_ekf_gsf));//初始化所有的过滤器模型
    const float yaw_increment = 2.0f * _m_pi / (float)N_MODELS_EKFGSF;//偏航角增量
    for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {//初始化每个过滤器模型,每个模型的偏航角都不一样,N_MODELS_EKFGSF为5
        // 在+-Pi之间均匀分配初始偏航角估计值
        _ekf_gsf[model_index].X(2) = -_m_pi + (0.5f * yaw_increment) + ((float)model_index * yaw_increment);

        // 取速度状态和与上次测量的相应方差
        _ekf_gsf[model_index].X(0) = _vel_NE(0);//速度东向
        _ekf_gsf[model_index].X(1) = _vel_NE(1);//速度北向
        _ekf_gsf[model_index].P(0,0) = sq(_vel_accuracy);//速度东向方差
        _ekf_gsf[model_index].P(1,1) = _ekf_gsf[model_index].P(0,0);//速度北向方差

        // 用半偏航间隔来表示偏航不确定性
        _ekf_gsf[model_index].P(2,2) = sq(0.5f * yaw_increment);//偏航角方差
    }
}

3.2 AHRS倾斜对齐

就是如何从增量速度计算旋转矩阵。旋转矩阵直接从加速度测量中构造，对于所有模型都是相同的，因此只需计算一次。其假设是：

偏航角为零 — 当速度融合开始时，每个模型的偏航角稍后对齐。
机体没有加速，因此所有测量的加速度都是由重力引起的。
用 \Delta t 表示传感器事件间隔时间。从 ECL EKF2 的主模块得到的加速度\boldsymbol{a}_{I} 计算得到增量速度 \boldsymbol{\Delta V}_{I}

\boldsymbol{\Delta V}_{I}=\boldsymbol{a}_{I}\cdot\Delta t

这是地球坐标系中的数据，需要转换到机体坐标系中并归一得到重力方向的单位向量

\boldsymbol{D}_{B}=-\boldsymbol{\Delta V}_{I}/\left\Vert \boldsymbol{\Delta V}_{I}\right\Vert

计算地球坐标系向北轴的单位向量，旋转为机体坐标系，与 \boldsymbol{D}_{B} 正交

\begin{array}{rl} \boldsymbol{N}_{B}&=\left[\begin{array}{c} 1\ 0\ 0 \end{array}\right]-\boldsymbol{D}_{B}\cdot\left(\left[\begin{array}{ccc} 1 & 0 & 0\end{array}\right]\boldsymbol{D}_{B}\right)\\boldsymbol{N}_{B}&=\boldsymbol{N}_{B}/\left\Vert \boldsymbol{N}_{B}\right\Vert \end{array}

计算地球坐标系向东轴的单位向量，旋转为机体坐标系，与 \boldsymbol{D}_{B} 和 \boldsymbol{N}_{B} 正交

\boldsymbol{E}_{B}=\boldsymbol{D}_{B}\times\boldsymbol{N}_{B}

从地球坐标系到机体坐标系的旋转矩阵中的每一列表示旋转到机体坐标系的相应地球坐标系单位向量的投影，例如 \boldsymbol{N}_{B} 将是第一列。我们需要从机体坐标系到地球坐标系的旋转矩阵，因此旋转到机体坐标系的地球坐标系单位向量被复制到相应的行中

\left[T\right]_{B}^{N}=\left[\begin{array}{c} \boldsymbol{N}_{B}^{\mathrm{T}}\ \boldsymbol{E}_{B}^{\mathrm{T}}\ \boldsymbol{D}_{B}^{\mathrm{T}} \end{array}\right]

倾斜对齐完成后可对每一个模型进行时间更新。

void EKFGSF_yaw::ahrsAlignTilt()//AHRS倾斜对齐
{
    // 旋转矩阵是直接从加速度测量中构建的，对于也是如此
    // 所有模型只需要计算一次假设是:
    //   1)偏航角为零——当速度融合开始时，每个模型的偏航角稍后对齐。
    //   2)车辆没有加速，因此所有测量的加速度都是由重力引起的。

    // 计算地球框架下轴单位矢量旋转到身体框架,归一得到重力方向的单位向量
    const Vector3f down_in_bf = -_delta_vel.normalized();

    // 计算大地框架旋转到车身框架的北轴单位矢量，正交于`down_in_bf `
    const Vector3f i_vec_bf(1.0f,0.0f,0.0f);
    Vector3f north_in_bf = i_vec_bf - down_in_bf * (i_vec_bf.dot(down_in_bf));
    north_in_bf.normalize();

    // 计算旋转到车身框架的地轴东轴单位矢量，正交于“down_in_bf”和“north_in_bf”
    const Vector3f east_in_bf = down_in_bf % north_in_bf;

    // 从地球框架到地球框架的旋转矩阵中的每一列表示的投影
    // 对应的地球框架单位向量旋转到身体框架，例如'north_in_bf'将是第一列。
    // 我们需要从body框架到earth框架的旋转矩阵，以便earth框架单位向量旋转到body
    // 将帧复制到相应的行中。
    Dcmf R;
    R.setRow(0, north_in_bf);
    R.setRow(1, east_in_bf);
    R.setRow(2, down_in_bf);

    for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
        _ahrs_ekf_gsf[model_index].R = R;
    }
}

偏航角对齐完成后可对每一个模型进行测量更新。

3.3 AHRS预测

在每次时间更新前先从 IMU 数据生成姿态基准，即每一个模型的姿态矩阵 \left[T\right]_{B}^{N} 被 IMU 数据驱动往前旋转了一个角度。所用数据为 IMU 数据及真实空速(固定翼飞机)，并用加速度融合增益系数和陀螺仪偏差增益系数进行计算。最后计算得到一个校正后的增量角度，将其应用到姿态矩阵 \left[T\right]_{B}^{N} 就得到新的姿态矩阵。

void EKFGSF_yaw::ahrsPredict(const uint8_t model_index)
{
    // 对所选模型使用简单的互补滤波器生成姿态解
    const Vector3f ang_rate = _delta_ang / fmaxf(_delta_ang_dt, 0.001f) - _ahrs_ekf_gsf[model_index].gyro_bias;

    const Dcmf R_to_body = _ahrs_ekf_gsf[model_index].R.transpose();// 旋转矩阵转置
    const Vector3f gravity_direction_bf = R_to_body.col(2);// 重力方向

    // 使用加速度数据进行角速度修正，并在加速度幅度偏离1g时减少修正(减少车辆拾取和移动时的漂移)。在固定翼飞行，补偿向心加速度假设协调转弯和X轴向前
    Vector3f tilt_correction;
    if (_ahrs_accel_fusion_gain > 0.0f) {// 如果加速度融合增益大于0

        Vector3f accel = _ahrs_accel;// 加速度

        if (_true_airspeed > FLT_EPSILON) {// 如果向心加速度补偿的真实空速大于0
            // 假设X轴与空速向量对齐，计算车体框架向心加速度，使用车体速率与车体向心加速度向量补偿的真实空速的叉积
            const Vector3f centripetal_accel_bf = Vector3f(0.0f, _true_airspeed * ang_rate(2), - _true_airspeed * ang_rate(1));

            // 正确测量的向心加速度
            accel -= centripetal_accel_bf;
        }

        tilt_correction = (gravity_direction_bf % accel) * _ahrs_accel_fusion_gain / _ahrs_accel_norm;// 重力方向与加速度取余，乘以加速度融合增益除以加速度的模

    }

    // 陀螺偏差估计
    constexpr float gyro_bias_limit = 0.05f;
    const float spinRate = ang_rate.length();
    if (spinRate < 0.175f) {// 如果旋转速率小于0.175
        _ahrs_ekf_gsf[model_index].gyro_bias -= tilt_correction * (_gyro_bias_gain * _delta_ang_dt);// 陀螺偏差减去倾斜修正乘以陀螺偏差增益乘以增量角度时间
        _ahrs_ekf_gsf[model_index].gyro_bias = matrix::constrain(_ahrs_ekf_gsf[model_index].gyro_bias, -gyro_bias_limit, gyro_bias_limit);// 限制陀螺偏差
    }

    // 前一帧与当前帧的角度差
    const Vector3f delta_angle_corrected = _delta_ang + (tilt_correction - _ahrs_ekf_gsf[model_index].gyro_bias) * _delta_ang_dt;// 增量角度加上倾斜修正减去陀螺偏差乘以增量角度时间

    // 对旋转矩阵应用角度
    _ahrs_ekf_gsf[model_index].R = ahrsPredictRotMat(_ahrs_ekf_gsf[model_index].R, delta_angle_corrected);

}

3.4 AHRS偏航角对齐

其算法是根据最新的偏航角 \psi 更新欧拉角向量，进而生成新的姿态矩阵 \left[T\right]_{B}^{N} 。在 24 参数 EKF 算法之外，在 EKF-GSF 算法中为每一个模型用增益系数法维护一个独立的姿态矩阵\left[T\right]_{B}^{N} ，用于后面的算法中。

void EKFGSF_yaw::ahrsAlignYaw()//AHRS偏航角对齐
{
    // 对齐每个模型的偏航角
    for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
        Dcmf& R = _ahrs_ekf_gsf[model_index].R;//获取ekf gsf的R,并传入
        const float yaw = wrap_pi(_ekf_gsf[model_index].X(2));//获取ekf gsf的yaw
        R = updateYawInRotMat(yaw, R);//基于yaw角更新R

        _ahrs_ekf_gsf[model_index].aligned = true;//对齐标志位
    }
}

3.5 AHRS更新

同样的，在每次测量更新之后，需要对姿态矩阵 \left[T\right]_{B}^{N} 进行更新。因为增量偏航角 \Delta\psi 发生在 xy 平面上，所以利用偏航旋转矩阵的稀疏性可以对姿态矩阵 \left[T\right]_{B}^{N} 做优化更新。

    // 利用偏航角旋转矩阵的稀疏性，将偏航角的变化应用于AHRS
    const float cosYaw = cosf(yawDelta);
    const float sinYaw = sinf(yawDelta);
    const float R_prev00 = _ahrs_ekf_gsf[model_index].R(0, 0);
    const float R_prev01 = _ahrs_ekf_gsf[model_index].R(0, 1);
    const float R_prev02 = _ahrs_ekf_gsf[model_index].R(0, 2);

    _ahrs_ekf_gsf[model_index].R(0, 0) = R_prev00 * cosYaw - _ahrs_ekf_gsf[model_index].R(1, 0) * sinYaw;//旋转矩阵R
    _ahrs_ekf_gsf[model_index].R(0, 1) = R_prev01 * cosYaw - _ahrs_ekf_gsf[model_index].R(1, 1) * sinYaw;
    _ahrs_ekf_gsf[model_index].R(0, 2) = R_prev02 * cosYaw - _ahrs_ekf_gsf[model_index].R(1, 2) * sinYaw;
    _ahrs_ekf_gsf[model_index].R(1, 0) = R_prev00 * sinYaw + _ahrs_ekf_gsf[model_index].R(1, 0) * cosYaw;
    _ahrs_ekf_gsf[model_index].R(1, 1) = R_prev01 * sinYaw + _ahrs_ekf_gsf[model_index].R(1, 1) * cosYaw;
    _ahrs_ekf_gsf[model_index].R(1, 2) = R_prev02 * sinYaw + _ahrs_ekf_gsf[model_index].R(1, 2) * cosYaw;

4. 偏航角预测方程

4.1 在预测中的变量意义

令 \psi 表示机体坐标系相对于地球坐标系的偏航角(yaw)。

令 \boldsymbol{V}_{NE} 表示机体在世界坐标系中的向北(N)和向东(E)的速度，

\boldsymbol{V}_{NE}=\left[\begin{array}{c} V_{N}\ V_{E} \end{array}\right]

令 \Delta\psi 表示 IMU 的 z 轴在机体轴上的增量角度测量值，即增量偏航角的测量值。

令 \sigma_{\Delta\psi}^{2} 表示 IMU 的 z 轴增量角度测量方差值。

令\boldsymbol{\Delta V}_{xy} 表示 IMU 的 x 轴和 y 轴在机体轴上的增量速度测量值

\boldsymbol{\Delta V}_{xy}=\left[\begin{array}{c} \Delta V_{x}\ \Delta V_{y} \end{array}\right]

令 \boldsymbol{\sigma}_{\boldsymbol{\Delta V}xy}^{2} 表示 IMU 的 x 轴和 y 轴增量速度测量方差

\boldsymbol{\sigma}_{\boldsymbol{\Delta V}xy}^{2}=\left[\begin{array}{c} \sigma_{\Delta V_{x}}^{2}\ \sigma_{\Delta V_{y}}^{2} \end{array}\right]

4.2 具体推导

推导机体到导航方向的变换矩阵(2D)

\left[T\right]_{B}^{N}=\left[\begin{array}{cc} \cos\left(\psi\right) & -\sin\left(\psi\right)\ \sin\left(\psi\right) & \cos\left(\psi\right) \end{array}\right]

    // 使用IMU数据生成姿态参考
    ahrsPredict(model_index);

    // 在有常规的速度观测值之前，我们不会开始运行算法的EKF部分
    if (!_ekf_gsf_vel_fuse_started) {
        return;
    }

    // 计算偏航状态使用投影到水平，以避免万向节锁定
    const Dcmf& R = _ahrs_ekf_gsf[model_index].R;
    _ekf_gsf[model_index].X(2) = shouldUse321RotationSequence(R) ?
                    getEuler321Yaw(R) :
                    getEuler312Yaw(R);//获取yaw角

    // 在水平的前-右框架中计算delta速度
    const Vector3f del_vel_NED = _ahrs_ekf_gsf[model_index].R * _delta_vel;//计算delta速度
    const float cos_yaw = cosf(_ekf_gsf[model_index].X(2));//计算cos(yaw)
    const float sin_yaw = sinf(_ekf_gsf[model_index].X(2));//计算sin(yaw)
    const float dvx =   del_vel_NED(0) * cos_yaw + del_vel_NED(1) * sin_yaw;//计算delta速度在x轴上的分量
    const float dvy = - del_vel_NED(0) * sin_yaw + del_vel_NED(1) * cos_yaw;//计算delta速度在y轴上的分量

姿态更新方程

\psi^{\prime}=\psi+\Delta\psi

速度更新方程

\boldsymbol{V}_{NE}{\prime}=\boldsymbol{V}_{NE}+\left[T\right]_{B}{N}\boldsymbol{\Delta V}_{xy}

    // 使用固定的速度增量和角度增量过程噪声方差
    const float dvxVar = sq(_accel_noise * _delta_vel_dt); // 前向速度变化量- (m/s)^2
    const float dvyVar = dvxVar; // 右速度变化量- (m/s)^2
    const float dazVar = sq(_gyro_noise * _delta_ang_dt); // 偏航的方差角- rad^2

定义状态向量

\hat{\boldsymbol{x}}_{k-1|k-1}=\left[\begin{array}{c} \boldsymbol{V}_{NE}\ \psi \end{array}\right]

定义过程方程的向量

\hat{\boldsymbol{x}}_{k|k-1}=\left[\begin{array}{c} \boldsymbol{V}_{NE}^{\prime}\ \psi^{\prime} \end{array}\right]

此即 EKF 的预测 (先验) 状态估计方程。

计算状态转移矩阵

\begin{array}{rl} \boldsymbol{F}&=\dfrac{\partial\hat{\boldsymbol{x}}_{k|k-1}}{\partial\hat{\boldsymbol{x}}_{k-1|k-1}}\&=\dfrac{\partial\left(\left[\begin{array}{c} V_{N}+\cos\left(\psi\right)\Delta V_{x}-\sin\left(\psi\right)\Delta V_{y}\ V_{E}+\sin\left(\psi\right)\Delta V_{x}+\cos\left(\psi\right)\Delta V_{y}\ \psi+\Delta\psi \end{array}\right]\right)}{\partial\left(\left[\begin{array}{c} V_{N}\ V_{E}\ \psi \end{array}\right]\right)}\&=\left[\begin{array}{ccc} 1 & 0 & -\sin\left(\psi\right)\Delta V_{x}-\cos\left(\psi\right)\Delta V_{y}\ 0 & 1 & \cos\left(\psi\right)\Delta V_{x}-\sin\left(\psi\right)\Delta V_{y}\ 0 & 0 & 1 \end{array}\right] \end{array}

定义控制(干扰)向量

\boldsymbol{u}=\left[\begin{array}{c} \boldsymbol{\Delta V}_{xy}\ \Delta\psi \end{array}\right]

推导协方差预测方程。在消除偏差效应后，假设惯性解中的误差增长由增量角度和增量速度中的“噪声”驱动。推导IMU噪声到状态噪声的 3\times3 控制(干扰)影响矩阵

\begin{array}{rl} \boldsymbol{G}&=\dfrac{\partial\hat{\boldsymbol{x}}_{k|k-1}}{\partial\boldsymbol{u}}\&=\dfrac{\partial\left(\left[\begin{array}{c} V_{N}+\cos\left(\psi\right)\Delta V_{x}-\sin\left(\psi\right)\Delta V_{y}\ V_{E}+\sin\left(\psi\right)\Delta V_{x}+\cos\left(\psi\right)\Delta V_{y}\ \psi+\Delta\psi \end{array}\right]\right)}{\partial\left(\left[\begin{array}{c} \Delta V_{x}\ \Delta V_{y}\ \Delta\psi \end{array}\right]\right)}\&=\left[\begin{array}{ccc} \cos\left(\psi\right) & -\sin\left(\psi\right) & 0\ \sin\left(\psi\right) & \cos\left(\psi\right) & 0\ 0 & 0 & 1 \end{array}\right] \end{array}

这实际上是在 xy 平面上的旋转矩阵。

定义干扰(disturbance)矩阵，即 3\times3 过程噪声矩阵

\boldsymbol{D}=\mathrm{diag}\left(\left[\begin{array}{c} \boldsymbol{\sigma}_{\boldsymbol{\Delta V}xy}^{2}\ \sigma_{\Delta\psi}^{2} \end{array}\right]\right)

推导状态误差矩阵，即过程噪声协方差矩阵

\boldsymbol{Q}=\boldsymbol{G}\boldsymbol{D}\boldsymbol{G}^{\mathrm{T}}

传播协方差矩阵

\boldsymbol{P}_{k|k-1}=\boldsymbol{F}\boldsymbol{P}_{k-1|k-1}\boldsymbol{F}^{\mathrm{T}}+\boldsymbol{Q}

此即 EKF 中的预测 (先验) 估计协方差方程。在代码中将这部分简化了

    // Local short variable name copies required for readability
    const float &P00 = _ekf_gsf[model_index].P(0,0);//设置上一时刻的P作为当前时刻的P的初始值
    const float &P01 = _ekf_gsf[model_index].P(0,1);
    const float &P02 = _ekf_gsf[model_index].P(0,2);
    const float &P11 = _ekf_gsf[model_index].P(1,1);
    const float &P12 = _ekf_gsf[model_index].P(1,2);
    const float &P22 = _ekf_gsf[model_index].P(2,2);
    const float &psi = _ekf_gsf[model_index].X(2);//获取yaw角

    // 使用固定的速度增量和角度增量过程噪声方差
    const float dvxVar = sq(_accel_noise * _delta_vel_dt); // 前向速度变化量- (m/s)^2
    const float dvyVar = dvxVar; // 右速度变化量- (m/s)^2
    const float dazVar = sq(_gyro_noise * _delta_ang_dt); // 偏航的方差角- rad^2

    // src/lib/ecl/EKF/python/ekf_derivation/main.py从SymPy脚本自动生成的优化代码
    const float S0 = cosf(psi);//计算cos(yaw)
    const float S1 = ecl::powf(S0, 2);//计算cos(yaw)^2
    const float S2 = sinf(psi);//计算sin(yaw)
    const float S3 = ecl::powf(S2, 2);//计算sin(yaw)^2
    const float S4 = S0*dvy + S2*dvx;//计算delta速度在x轴上的分量
    const float S5 = P02 - P22*S4;//计算P(0,2)-P(2,2)*S4
    const float S6 = S0*dvx - S2*dvy;//计算delta速度在y轴上的分量
    const float S7 = S0*S2;//计算cos(yaw)*sin(yaw)
    const float S8 = P01 + S7*dvxVar - S7*dvyVar;//计算P(0,1)+S7*dvxVar-S7*dvyVar
    const float S9 = P12 + P22*S6;//计算P(1,2)+P(2,2)*S6

    // 对于P的求解
    _ekf_gsf[model_index].P(0,0) = P00 - P02*S4 + S1*dvxVar + S3*dvyVar - S4*S5;
    _ekf_gsf[model_index].P(0,1) = -P12*S4 + S5*S6 + S8;
    _ekf_gsf[model_index].P(1,1) = P11 + P12*S6 + S1*dvyVar + S3*dvxVar + S6*S9;
    _ekf_gsf[model_index].P(0,2) = S5;
    _ekf_gsf[model_index].P(1,2) = S9;
    _ekf_gsf[model_index].P(2,2) = P22 + dazVar;

    // 协方差矩阵是对称的，所以将上半部分复制到下半部分
    _ekf_gsf[model_index].P(1,0) = _ekf_gsf[model_index].P(0,1);
    _ekf_gsf[model_index].P(2,0) = _ekf_gsf[model_index].P(0,2);
    _ekf_gsf[model_index].P(2,1) = _ekf_gsf[model_index].P(1,2);

    // constrain variances
    const float min_var = 1e-6f;
    for (unsigned index = 0; index < 3; index++) {//对P的对角元素进行约束
        _ekf_gsf[model_index].P(index,index) = fmaxf(_ekf_gsf[model_index].P(index,index),min_var);//对角线元素取最大值
    }

5. 偏航估计器的观测方程

由 GPS 提供的向北(N)、向东(E)速度观测值为

\boldsymbol{V}_{GPS}=\left[\begin{array}{c} V_{N_{GPS}}\ V_{E_{GPS}} \end{array}\right]

其方差为 \sigma_{\Delta V_{GPS}}^{2} ，由此构建 2\times2 测量噪声矩阵

\boldsymbol{R}=\mathrm{diag}\left(\left[\begin{array}{c} \sigma_{\Delta V_{GPS}}^{2}\ \sigma_{\Delta V_{GPS}}^{2} \end{array}\right]\right)

因为是对速度的直接观测，所以测量矩阵比较简单

\boldsymbol{H}=\left[\begin{array}{ccc} 1 & 0 & 0\ 0 & 1 & 0 \end{array}\right]

推导向北(N)、向东(E)速度观测的协方差更新方程，即 EKF 的计算最佳卡尔曼增益方程、计算更新 (后验) 的状态估计方程和计算更新 (后验) 的协方差方程

\begin{array}{rl} \tilde{\boldsymbol{y}}&=\boldsymbol{H}\hat{\boldsymbol{x}}_{k|k-1}-\boldsymbol{V}_{GPS}\\boldsymbol{S}&=\boldsymbol{H}\boldsymbol{P}_{k|k-1}\boldsymbol{H}^{\mathrm{T}}+\boldsymbol{R}\\boldsymbol{K}&=\boldsymbol{P}_{k|k-1}\boldsymbol{H}^{\mathrm{T}}\boldsymbol{S}{-1}\\hat{\boldsymbol{x}}_{k|k}&=\hat{\boldsymbol{x}}_{k|k-1}-\boldsymbol{K}\tilde{\boldsymbol{y}}\\boldsymbol{P}_{k|k}&=\boldsymbol{P}_{k|k-1}-\boldsymbol{K}\boldsymbol{S}\boldsymbol{K}^{\mathrm{T}} \end{array}

// 使用速度测量值更新指定模型指标的EKF状态和协方差
bool EKFGSF_yaw::updateEKF(const uint8_t model_index)
{
    // 从GPS提供的精度估计中设置观测方差，并应用健全检查最小值
    const float velObsVar = sq(fmaxf(_vel_accuracy, 0.01f));

    // 计算速度观测
    _ekf_gsf[model_index].innov(0) = _ekf_gsf[model_index].X(0) - _vel_NE(0);//计算速度观测值
    _ekf_gsf[model_index].innov(1) = _ekf_gsf[model_index].X(1) - _vel_NE(1);

    // Use temporary variables for covariance elements to reduce verbosity of auto-code expressions
    const float &P00 = _ekf_gsf[model_index].P(0,0);//经过预测更新得到的P
    const float &P01 = _ekf_gsf[model_index].P(0,1);
    const float &P02 = _ekf_gsf[model_index].P(0,2);
    const float &P11 = _ekf_gsf[model_index].P(1,1);
    const float &P12 = _ekf_gsf[model_index].P(1,2);
    const float &P22 = _ekf_gsf[model_index].P(2,2);

    // optimized auto generated code from SymPy script src/lib/ecl/EKF/python/ekf_derivation/main.py
    const float t0 = ecl::powf(P01, 2);//计算P(0,1)^2
    const float t1 = -t0;//计算-P(0,1)^2
    const float t2 = P00*P11 + P00*velObsVar + P11*velObsVar + t1 + ecl::powf(velObsVar, 2);
    if (fabsf(t2) < 1e-6f) {
        return false;
    }
    const float t3 = 1.0F/t2;
    const float t4 = P11 + velObsVar;
    const float t5 = P01*t3;
    const float t6 = -t5;
    const float t7 = P00 + velObsVar;
    const float t8 = P00*t4 + t1;
    const float t9 = t5*velObsVar;
    const float t10 = P11*t7;
    const float t11 = t1 + t10;
    const float t12 = P01*P12;
    const float t13 = P02*t4;
    const float t14 = P01*P02;
    const float t15 = P12*t7;
    const float t16 = t0*velObsVar;
    const float t17 = ecl::powf(t2, -2);
    const float t18 = t4*velObsVar + t8;
    const float t19 = t17*t18;
    const float t20 = t17*(t16 + t7*t8);
    const float t21 = t0 - t10;
    const float t22 = t17*t21;
    const float t23 = t14 - t15;
    const float t24 = P01*t23;
    const float t25 = t12 - t13;
    const float t26 = t16 - t21*t4;
    const float t27 = t17*t26;
    const float t28 = t11 + t7*velObsVar;
    const float t30 = t17*t28;
    const float t31 = P01*t25;
    const float t32 = t23*t4 + t31;
    const float t33 = t17*t32;
    const float t35 = t24 + t25*t7;
    const float t36 = t17*t35;

    _ekf_gsf[model_index].S_det_inverse = t3;//计算S的逆

    _ekf_gsf[model_index].S_inverse(0,0) = t3*t4;//计算S的逆
    _ekf_gsf[model_index].S_inverse(0,1) = t6;
    _ekf_gsf[model_index].S_inverse(1,1) = t3*t7;
    _ekf_gsf[model_index].S_inverse(1,0) = _ekf_gsf[model_index].S_inverse(0,1);//S的逆

    matrix::Matrix<float, 3, 2> K;//根据S，计算K
    K(0,0) = t3*t8;
    K(1,0) = t9;
    K(2,0) = t3*(-t12 + t13);
    K(0,1) = t9;
    K(1,1) = t11*t3;
    K(2,1) = t3*(-t14 + t15);

    _ekf_gsf[model_index].P(0,0) = P00 - t16*t19 - t20*t8;
    _ekf_gsf[model_index].P(0,1) = P01*(t18*t22 - t20*velObsVar + 1);
    _ekf_gsf[model_index].P(1,1) = P11 - t16*t30 + t22*t26;
    _ekf_gsf[model_index].P(0,2) = P02 + t19*t24 + t20*t25;
    _ekf_gsf[model_index].P(1,2) = P12 + t23*t27 + t30*t31;
    _ekf_gsf[model_index].P(2,2) = P22 - t23*t33 - t25*t36;
    _ekf_gsf[model_index].P(1,0) = _ekf_gsf[model_index].P(0,1);
    _ekf_gsf[model_index].P(2,0) = _ekf_gsf[model_index].P(0,2);
    _ekf_gsf[model_index].P(2,1) = _ekf_gsf[model_index].P(1,2);

    // constrain variances
    const float min_var = 1e-6f;//最小方差
    for (unsigned index = 0; index < 3; index++) {
        _ekf_gsf[model_index].P(index,index) = fmaxf(_ekf_gsf[model_index].P(index,index),min_var);//对角线元素最小为min_var
    }

    // test ratio = transpose(innovation) * inverse(innovation variance) * innovation = [1x2] * [2,2] * [2,1] = [1,1]
    const float test_ratio = _ekf_gsf[model_index].innov * (_ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov);//计算innovation的转置*inverse(innovation variance)*innovation

    // 执行卡方创新一致性测试，并计算一个压缩比例因子，将创新的大小限制在5-Sigma。如果测试比率大于25 (5 Sigma)，则减少创新向量的长度，使其在5-Sigma his保护下免受较大的测量峰值
    const float innov_comp_scale_factor = test_ratio > 25.f ? sqrtf(25.0f / test_ratio) : 1.f;

    // 修正状态矢量并捕获偏航角的变化
    const float oldYaw = _ekf_gsf[model_index].X(2);

    _ekf_gsf[model_index].X -= (K * _ekf_gsf[model_index].innov) * innov_comp_scale_factor;//修正状态矢量

    const float yawDelta = _ekf_gsf[model_index].X(2) - oldYaw;//计算偏航角的变化

6. 偏航估计器的GSF方程

对每个模型的状态计算高斯密度。首先计算马式距离(Mahalanobis distance)

D_{M}^{2}=\tilde{\boldsymbol{y}}{\mathrm{T}}\boldsymbol{S}^{-1}\tilde{\boldsymbol{y}}

再计算2D正态分布(Multivariate Normal Distribution)的密度

\mathrm{Density}=\left(\dfrac{1}{2}\right)^{p/2}\sqrt{\det\left(\boldsymbol{S}{-1}\right)}\exp\left(-\dfrac{D_{M}^{2}}{2}\right)

其中我们观测的是 2\times1 速度向量，所以 p=2 。

计算 5 组 AHRS 的解的权重并归一

\begin{array}{rl} W_{i}^{\prime}&=\mathrm{Density}_{i}\cdot W_{i}\\boldsymbol{W}&=\boldsymbol{W}^{\prime}/\left\Vert \boldsymbol{W}^{\prime}\right\Vert \end{array}

用每个模型的偏航状态的加权平均值以计算复合偏航向量。为避免角度回绕问题，在求和之前，将偏航状态转换为长度等于权重值为 1 的向量

\begin{array}{rl} x&=\sum_{i=1}^{5}W_{i}\cdot\cos\left(\psi_{i}\right)\y&=\sum_{i=1}^{5}W_{i}\cdot\sin\left(\psi_{i}\right)\\psi_{GSF}&=\mathrm{atan2}\left(y,x\right) \end{array}

该算法原理在于将每一个偏航状态\psi_{i} 看成是一个从原点出发的向量，因而在 x/y 轴上的投影通过权重缩放后相加，就得到单位圆上的一个向量，因而求出加权复合偏航角 \psi_{GSF} 。

根据每个模型方差的加权平均值计算偏航状态的复合方差。具有较大新息的模型的权重较小

\begin{array}{rl} \Delta\psi_{i}&=\psi_{i}-\psi_{GSF}\\mathrm{variance}_{\psi_{GSF}}&=\sum_{i=1}^{5}W_{i}\cdot\left(P_{i}\left(3,3\right)+\Delta\psi_{i}^{2}\right)\end{array}

其中 P_{i}\left(3,3\right) 为第 i 组的协方差矩阵 \boldsymbol{P}_{i} 中偏航角 \psi_{i}的方差。

// 三态EKF模型仅在飞行时运行，以避免由于操作员处理和GPS干扰而损坏的估计
    if (_run_ekf_gsf && _vel_data_updated) {
        if (!_ekf_gsf_vel_fuse_started) {
            initialiseEKFGSF();// 初始化
            ahrsAlignYaw();
            // Initialise to gyro bias estimate from main filter because there could be a large
            // uncorrected rate gyro bias error about the gravity vector
            for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
                _ahrs_ekf_gsf[model_index].gyro_bias = imu_gyro_bias;
            }
            _ekf_gsf_vel_fuse_started = true;
        } else {
            bool bad_update = false;
            for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
                // 随后的测量值被融合为直接状态观测值
                if (!updateEKF(model_index)) {// 更新
                    bad_update = true;
                }
            }

            if (!bad_update) {// 如果更新成功
                float total_weight = 0.0f;// 权重总和
                // 假设每个模型服从正态分布，计算其权重
                const float min_weight = 1e-5f;// 最小权重
                uint8_t n_weight_clips = 0;// 权重剪切次数
                for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {// 对每个模型进行权重计算
                    _model_weights(model_index) = gaussianDensity(model_index) * _model_weights(model_index);// 根据高斯分布和上一时刻模型权重计算最新的权重
                    if (_model_weights(model_index) < min_weight) {// 如果权重小于最小权重
                        n_weight_clips++;// 剪切次数加1
                        _model_weights(model_index) = min_weight;// 权重设为最小权重
                    }
                    total_weight += _model_weights(model_index);// 权重总和
                }

                // 归一化加权函数
                if (n_weight_clips < N_MODELS_EKFGSF) {// 如果剪切次数小于模型总数
                    _model_weights /= total_weight;// 权重归一化
                } else {
                    // 由于过度的更新方差，所有权重都已崩溃，因此需要重新设置滤波器
                    initialiseEKFGSF();
                }
            }
        }
    } else if (_ekf_gsf_vel_fuse_started && !_run_ekf_gsf) {
        // wait to fly again
        _ekf_gsf_vel_fuse_started = false;
    }

参考链接

https://zhuanlan.zhihu.com/p/428920320

https://docs.px4.io/v1.12/en/advanced_config/tuning_the_ecl_ekf.html?