组合导航ESKF关键知识点：姿态计算&观测校正

原创

已于 2023-04-16 10:43:45 修改 · 866 阅读

1 ·

CC 4.0 BY-SA版权

文章标签：

#自动驾驶

于 2023-04-16 01:21:51 首次发布

文章详细阐述了姿态计算的不同方法，包括在ESKF（扩展卡尔曼滤波）中使用等效旋转矢量进行propagation和update过程，以及在observation过程中如何利用四元数或姿态阵计算姿态误差。校正阶段则涉及四元数的乘法运算来调整姿态。文章还探讨了四种不同的观测与校正配合方式，确保姿态量的正确更新。

1.姿态计算

方法：等效旋转矢量为小量时才可以使用加减法；四元数和姿态阵采用乘法；欧拉角只有物理意义不直接用于计算。
ESKF中状态量的建立使用等效旋转矢量；ESKF 的prediction和update过程使用等效旋转矢量做运算；

1.1 progation过程姿态变化量采用等效旋转矢量计算

因为短时间内姿态变化量是小量，可以与陀螺转动积分的结果，在delta_t小段时间内做代数运算。以初始时刻姿态为基准,求出姿态变化量等效旋转矢量，转化为姿态变化量四元数，求出新时刻姿态先验值。
$ϕk+1=θk+1+112θk×θk+1Δqk+1k=rotateVector2quaternion(ϕk)qk+1nav_prior=qk∗Δqk+1k \phi_{k+1}=\theta_{k+1}+\frac{1}{12}\theta_{k}\times\theta_{k+1} \newline \Delta{q^k_{k+1}} = rotateVector2quaternion(\phi_k) \newline q^{nav}_{k+1}\_prior = q_{k}*\Delta{q^k_{k+1}}$