|
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)速度和偏航角。偏航角估算开始时的角度间隔相等，
[
−
4
/
5
π
−
2
/
5
π
0
2
/
5
π
4
/
5
π
]
\left[\begin{array}{ccccc} -4/5\pi & -2/5\pi & 0 & 2/5\pi & 4/5\pi\end{array}\right]
[−4/5π−2/5π02/5π4/5π] 。GPS的向北(N)、向东(E)速度作为观测值。 高斯和滤波器(Gaussian Sum Filter)
根据标准化 GPS 的向北(N)、向东(E)速度新息数值级别计算每个 EKF 的权重。总权重为 1 。输出偏航角
ψ
\psi
ψ 估计值，这是单个 EKF 估计值的加权平均值。
GSF-EKF过程框图如下：  在我们拿到ulg文件后发现：
vehicle_angular_velocity大概45~50hzvehicle_attitude大概20hz 这就代表我们IMU的更新频率并不是很高，但是取得了比较好的收益 
2. 偏航估计器的算法直觉
EKF-GSF 算法是一个很新奇也很有创意的算法。首先该算法一个假设就是偏航角由 xy 水平面的向北(N)、向东(E)的增量速度和绕 z 轴旋转的增量角度所驱动，所以偏航角的测量误差也和这 3 个值的测量噪声相关
ψ
e
r
r
o
r
∝
(
σ
Δ
V
x
2
,
σ
Δ
V
y
2
,
σ
Δ
ψ
2
)
\psi_{error}\propto\left(\sigma_{\Delta V_{x}}^{2},\sigma_{\Delta V_{y}}{2},\sigma_{\Delta\psi}{2}\right)
ψerror∝(σΔVx2,σΔVy2,σΔψ2)
这 3 个值和 3 个噪声由 IMU 提供。于是我们可以在 xy 水平面上面预置 5 个偏航角
ψ
i
,
i
∈
1
∼
5
\psi_{i},i\in1\sim5
ψi,i∈1∼5 ，每个偏航角由这 3 个值和 3 个噪声驱动演化，独立进行时间更新。在测量更新阶段，我们观测 GPS 提供的 xy 水平面的向北(N)、向东(E)的速度，因为残差的存在，这 5 个偏航角
ψ
i
\psi_{i}
ψi 会向真实的偏航角
ψ
t
r
u
e
\psi_{true}
ψtrue 聚拢，但是真实的偏航角
ψ
t
r
u
e
\psi_{true}
ψtrue 不可见。
不过我们可以通过高斯和的算法，计算这 5 个偏航角
ψ
i
\psi_{i}
ψi的权重并相加，得到距离真实的偏航角
ψ
t
r
u
e
\psi_{true}
ψtrue 最近的复合偏航角
ψ
G
S
F
\psi_{GSF}
ψ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
// 旋转矩阵是直接从加速度测量中构建的，对于也是如此
// 所有模型只需要计算一次假设是:
// 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
// 对所选模型使用简单的互补滤波器生成姿态解
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
// 对齐每个模型的偏航角
for (uint8_t model_index = 0; model_index |