基于四元数的扩展卡尔曼滤波器（EKF）姿态估计文献小结

欢迎交流~ 个人 Gitter 交流平台，点击直达：

Reference

1. Survey of Nonlinear Attitude Estimation Methods →​ 2007

   综述性文章

   This paper will review the basic assumptions of these filters,observers, and smoothers（平滑器）, presenting enough mathematical detail to give a general orientation. First, reviews of the quaternion parameterization and gyro model equations are given. Then, attitude estimation methods based on the EKF are shown, followed by QUEST-based approaches. Next, the two-step estimator is shown.The UF and PF approaches are then shown, followed by the orthogonal filter. Then, the predictive filter, as well as nonlinear observers and adaptive approaches are reviewed. The paper concludes with a discussion of the strengths and weaknesses of the various filters.

   本文回顾了一些滤波器（filters）、观测器（observers）、平滑器（smoothers）的基本假设，展现了足够的数学推导以给出一个大致的方位

   姿态估计分两步：首先通过测量机体来估计飞行器的方位，然后对测量噪声进行过滤。

   Kinematics Model 运动学模型：

   q˙=12⋅q[w]×

   QUEST（QUaternion ESTimator）

   UF（Unscented Filter）无迹滤波器

   PF（Particle Filters）粒子滤波器

   Adaptive approaches 自适应方法

   recently proposed filter 最近提出

   Nonlinear observers often exhibit global convergence, which is to say 非线性观测器通常表现出全局稳定性，这就是说……

   Least-squares 最小二乘

   polynomials 多项式

   Nonlinear adaptive techniques are similar to nonlinear observers in that they usually provide global stability proofs that guarantee convergence of the estimated parameters 非线性自适应技术类似于非线性观测器，它们通常提供保证估计参数收敛的全局稳定性证明……

   convention 规定，惯例，习俗

   informally 通俗的说

   quaternion conjugation 共轭四元数

   quaternion multiplication

2. Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs → 2015

   将四元数解耦成倾斜、旋转两部分，以避免磁干扰对横滚、俯仰分量造成影响。

   In this article, we propose a deterministic approach to solve Whaba’s problem given gravity and magnetic field observations provided by the MARG sensor. This method returns an estimation of the attitude in quaternion form without leading to ambiguous results and avoiding singularity problems.Moreover, it does not need a predefined knowledge of the magnetic field direction. We also propose a new approach to a complementary filter for fusing together gyroscope data with acceleration and magnetic field measurements from IMU and MARG sensors to obtain an orientation estimation in quaternion form. This algorithm is meant to be used onboard MAVs because of its low computational burden, robustness, and fast convergence.

3. An Introduction to the Kalman Filter → 2006

   给出了卡尔曼滤波器的总体性概述，然后讨论方程式的具体细节及其作用。

   卡尔曼滤波器可分为两个部分：时间更新方程和测量更新方程。时间更新方程负责及时向前推算当前状态变量和误差协方差估计的值，以便为下一个时间状态构造先验估计。测量更新方程负责反馈——也就是说，它将先验估计和新的测量变量结合以构造改进的后验估计。时间更新方程也可视为预估方程，测量更新方程可视为校正方程。

4. Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation → 2012

   用标量的数学表示方法、基本的代数运算以及一种浅显易懂的思想实验方法对卡尔曼滤波器进行了简单的推导。卡尔曼滤波器的递归过程实质上是由高斯概率分布函数相乘体现出来的。

5. 基于四元数非线性滤波的飞行器姿态确定算法研究 → 2011 博士论文

   确定性算法就是如何根据某一时刻的一组测量信息求解出此时刻飞行器的姿态矩阵。其存在的条件是当前时刻具有至少两个独立的观测矢量。典型的确定性算法主要包括TRIAD算法和基于Wahba问题求解的QUEST法、SVD法和FOAM法等。

   四元数因为计算量小，非奇异性和可全姿态工作等优点而广泛应用于姿态确定算法
   当中。但是基于四元数的非线性滤波算法都不可避免的会遇到四元数规范化问题和协方差奇异性问题。

   **卡尔曼滤波**KF是在系统状态空间模型的基础上，以最小均方误差为准则，一建立的一个线性无偏、最小方差递推滤波器

   **扩展卡尔曼滤波器**EKF采用泰勒级数展开方法将非线性系统线性化，进而采用kalman的基本步骤进行状态估计更新。EKF算法虽已获得广泛应用，但是它存在两个无法克服的缺点：其一，对于较强的非线性系统，容易引入较大的线性化误差；其二，必须计算Jacobian矩阵，比较繁琐。这大大限制了它在非线性算法中的应用。

6. 小型四旋翼飞行器导航与控制系统研究 → 2013 硕士论文

   基于四元数的扩展卡尔曼滤波算法实现：首先根据三轴陀螺仪传感器的测量值对机体姿态角、航向角进行估计，再根据三轴加速度传感器、三轴磁阻传感器的测量值对机体姿态角、航向角度进行估计，然后通过扩展卡尔曼滤波算法利用两次得到的姿态角估计值对四元数、陀螺仪零偏进行修正，再根据修正后的四元数、陀螺仪零点漂移至计算出最终的机体姿态、航向角度。

   对EKF方程系数的感性认识：量测更新是对时间更新值的修正，修正量由时间更新的质量优劣( Pk|k−i ),量测信息的质量优劣（ Rk ）、量测与状态的关系（ Hk ）以及具体的测量值 Zk 确定，量测更新方程建立的目的是正确合理的利用量测值。

   卡尔曼滤波增益矩阵 Kk 决定了对量测值 Zk 和上一步 X^k−1 利用的比例程度

7. An improved quaternion Gauss–Newton algorithm for attitude determination using magnetometer and accelerometer → 2014

   a new measurement model for vector sensors is proposed based on first-order Taylor series expansion and new statistical characteristic of measurement noise.

   The Gauss–Newton method transforms the nonlinear parameter estimation problem into a linear least squares recursive estimation, which results in a suboptimal solution。Nevertheless,the final results are unbiased estimation.

   提出了一种加速度计、磁力计的建模方法，使用G-N法。

   四元数 q=[q1q2q3q3]T=[q1ρ]T

   从机体坐标系到地球坐标系的姿态转换矩阵：

   A=(q21−ρTρ)I+2ρρT+2q1[ρ×]

8. Quaternion-Based Extended Kalman Filter for Determining Orientation by Inertial and Magnetic Sensing →2006

   In this paper, a quaternion based extended Kalman filter (EKF) is developed for determining the orientation of a rigid body from the outputs of a sensor which is configured as the integration of a tri-axis gyro and an aiding system mechanized using a tri-axis accelerometer and a tri-axis magnetometer.

   传感器模型：

   ⎧⎩⎨⎪⎪ω⃗ =ωtrue+gb+v⃗ Cbn(q)(g⃗ +a⃗ body+ab+v⃗ )Cbn(q)h⃗ +mb+v⃗ 

   状态量四元数、加速度计偏移、陀螺仪偏移；观测量加速度计和磁力计读数。

   加速度计和磁力计的使用进行了有效性验证，检测测得值与地理系的向量偏差进行处理以便后续使用。（测量噪声矩阵）

   当前时刻的先验估计(a priori estimate)未使用量测值的状态估计，使用了当前量测值的状态估计称为后验估计（a posteriori estimate）

   zero-mean white noise processes /zero-mean, Gaussian white noise 零均值白噪声

   quaternion time-evolution 四元数随时间更新

   first-order approximation 一阶近似

9. Discrete-Time Complementary Filters for Attitude and Position Estimation: Design, Analysis and Experimental Validation → 2011

   This paper develops a navigation system based on complementary filtering for position and attitude estimation, with application to autonomous surface crafts. Using strapdown inertial measurements, vector observations, and global positioning system (GPS) aiding, the proposed complementary filters provide attitude estimates in Euler angles representation and position
   estimates in Earth frame coordinates, while compensating for rate gyro bias.

   the attitude is estimated by exploiting the angular velocity and attitude measurements provided
   by strap-down sensors.

10. Application of Quaternion-Based Extended Kalman Filter for MAV Attitude Estimation Using MEMS Sensors → 2009

    a new quaternion-based extended Kalman filter algorithm was proposed to improve the accuracy of attitude estimation of micro aerial vehicles based on the MEMS sensors such as gyroscope, accelerometer and magnetometer.Attitude quaternion errors and drift bias of gyroscope were selected to construct a state vector，and the state equation was established based on attitude quaternion error differential equation and stochastic error model of gyroscope. The modified Gauss-Newton algorithm was used to convert the outputs of sensors to quaternion by which the measurements of Kalman filter were obtained through making combination of attitude quaternion from gyroscope signals

11. A Simplified Quaternion-Based Algorithm for Orientation Estimation From Earth Gravity and Magnetic Field Measurements → 2008

    1)derivation of a new geometrically intuitive algorithm for determining orientation that is relative to an Earth-fixed reference frame based on a set of accelerometer and magnetometer measurements;
    2) a singularity avoidance method that allows the algorithm to track through all orientations;

12. Fast Complementary Filter for Attitude Estimation Using Low-Cost MARG Sensors

    → 2016

    a fixed-gain complementary filter is designed fusing related sensors. To avoid using iterative algorithms, the accelerometer-based attitude determination is transformed into a linear system. Stable solution to this system is obtained via control theory.To decrease significant effects of unknown magnetic distortion imposing on the **magnetometer,**a stepwise filtering architecture is designed. The magnetic output is fused with the estimated gravity from gyroscope and accelerometer using a second complementary filter when there is no significant magnetic distortion.

13. Fast Quaternion Attitude Estimation from Two Vector Measurements → 2000

    Create two methods to obtain the quaternion from gravity and magnetometer:

    • Optimal Two-Observation Quaternion Estimation Method- Suboptimal Two
    • Observation Quaternion Estimation Method

14. Quaternion Attitude Estimation Using Vector Observations → 1999

    This paper contains a critical comparison of estimators minimizing Wahba’s loss function.

    It is well known that two vectors are sufficient to determine the attitude;and the method of Farrell and Stuelpnagel, as well as the other methods described in this paper, only require B to have rank two.

    Davenport provided the first useful solution of Wahba’s problem for spacecraft attitude determination,He parameterized the attitude matrix by a unit quaternion.Davenport’s q method avoids this singularity( (qopt)scale=0 ) by treating the four components symmetrically, but some other methods have singularities similar to that in QUEST,These singularities can be avoided by solving for the attitude with respect to a reference coordinate frame related to the original frame by 180° rotations about the x, y, or z coordinate axis

15. A Simplified Quaternion-Based Algorithm for Orientation Estimation From Earth Gravity and Magnetic Field Measurements → 2008

    Orientation of a static or slow-moving rigid body can be determined from the measured gravity and local magnetic field vectors.

    This paper presents a geometrically intuitive (几何直观)3-degree-of-freedom (3-DOF) orientation estimation algorithm with physical meaning [which is called the factored quaternion algorithm (FQA)], which restricts the use of magnetic data to the determination of the rotation about the vertical axis. 磁力计的数据只会对竖直方向上的转动产生影响。

    A singularity avoidance method is introduced,which allows the algorithm to track through all orientations.

    The TRIAD algorithm is a single frame deterministic method for solving Wahba’s problem. It requires normalized measurements of two nonparallel reference vectors as input.It produces a suboptimal orientation estimate in the form of a 3 × 3 rotation matrix.The algorithm constructs two triads of orthogonal unit vectors. The two triads are the components of an inertial frame expressed in both the body and Earth-fixed reference frames.

    The QUaternion ESTimator (QUEST) algorithm is a popular algorithm for single-frame estimation of a quaternion that represents the attitude of a rigid body relative to a fixed coordinate system. The algorithm was created to solve Wahba’s problem in the context of spacecraft attitude determination.Given a set of 3-D known reference unit vectors V1, V2, … , Vn and a set of the corresponding observation or measurement unit vectors W1,W2, … , Wn, Wahba’s problem is to find the least squares estimate of spacecraft attitude by minimizing the following loss function:

    L(A)=12∑i=1nai(Wi−AVi)T(Wi−AVi)

    A represents the orientation matrix , a1,a2,...an are nonnegative weighting coefficients.

    Davenport introduced a method of parameterizing the orientation matrix by a unit quaternion q and proved that the loss function above can be transformed into a quadratic gain function of the unit quaternion in the form of: $

    G(A(q))=∑ni=1ai−L(A(q)=qTKq

    K is a 4×4 matrix constructed from the reference vectors Vi, measurement vectorsWi,and weighting coefficients ai , i = 1, 2, … , n.

    In this paper,In the FQA, local magnetic field data are used only in azimuth angle calculations. This decoupling of accelerometer and magnetometer data eliminates the influence of magnetic variations on calculations that determine pitch and roll.

16. An Extended Kalman Filter for Quaternion-Based Orientation Estimation Using MARG Sensors → 2001

    Presents an extended Kalman filter(EKF) for real-time estimation of rigid body orientation using the newly developed MARG sensors.

    The Gauss-Newton iteration algorithm is utilized to find the best quaternion that relates the measured accelerations and earth magnetic field in the body coordinate frame to calculated values in the earth coordinate frame.

    one can expect that there exists a quaternion relating the measurements (values in body frame) to the real magnetic and gravity fields (values in earth frame).Obviously there are several sources of errors,including:

    ①misalignments between pairs of axes in each sensor;

    ②linear acceleration misinterpreted as gravity;

    ③variation of both gravity and magnetic field;

    ④errors inherent to the sensors.

    This paper examines this problem using the minimum-squared-error (MSE) criterion.
    需要准确的固定的重力向量和地磁向量。

17. 捷联式惯导系统初始对准方法研究 → 2006 博士论文
    捷联式惯导系统的关键技术包括初始对准问题、有害加速度的消除及引力修正、惯性元件误差模型的建立和实时补偿、捷联矩阵的更新等。

    初始对准分为两个阶段：第一阶段为粗对准,第二阶段为精对准。捷联系统粗对准的任务是得到粗略的捷联矩阵,为后续的精对准提供基础,此阶段精度可以低一些,但要求速度快。精对准是在粗对准的基础上进行的,通过处理惯性敏感元件的输出信息,精确校正真实导航坐标系与计算的导航坐标系之间的失准角,使之趋于零,从而得到精确的捷联矩阵。

    R.E.Kalman最初提出的滤波基本理论只适用于线性系统,并且要求观测方程也必须是线性的。扩展卡尔曼滤波(EKF)是一种应用最广泛的非线性滤波方法。EKF采用围绕上一步估计值 X^k 将非线性函数 f[⋅] 和 h[⋅] 展成泰勒级数,并取一阶近似来进行线性化的方法,得到非线性系统的线性化模型,再利用滤波递推方程进行系统的状态估计。当系统具有较强的非线性或初始误差较大时,滤波精度就会明显下降,甚至造成滤波发散。

    捷联系统的工作原理是,加速度计和陀螺测得载体坐标系相对于惯性坐标系的加速度、姿态角速度沿载体坐标系各轴的分量,利用陀螺测得的角速度信息计算出姿态矩阵并提取姿态角,利用姿态矩阵将加速度计的测量信息变换到导航坐标系上,然后进行加速度到速度的积分以及速度到位置的积分。

    粗对准的原理就是利用重力加速度和地球自转角速度在导航坐标系上的投影与它们在载体坐标系上的投影之间的坐标转换关系,来计算初始姿态矩阵的。对于微惯性导航系统来说，它采用的是3轴MEMS加速度计和3轴MEMS陀螺仪的仪表组合,其MEMS陀螺仪的精度较低。当惯性导航系统中使用中低精度的陀螺仪时,不能直接使陀螺仪的数据来进行初始对淮.因为在载体静基座或者匀速直线航向的情况下,这种精度的陀螺仪由于自身误差太大，不能正确地敏感地球自转角速度,虽然加速度计的精度满足对准，但是仅仅由加速度计数据是不能完成初始对准的.因此微惯导系统的MEMS加速度计和MEMS陀螺仪要与外部测量设备辅助组合才能完成初始对准。当下MEMS惯性测量单元与GPS、磁强计或者磁罗盘等的组合比较热门。这些组合在对准精度和速度上都能满足要求。

18. Attitude Estimation for Small Helicopter Using Extended Kalman Filter → 2008

    introduces a method based on EKF to estimate small helicopter’s attitude with little drift and low noise.

    观测方程非线性关系太强

19. Indirect Kalman Filter for 3D Attitude Estimation → 2005

    A Tutorial for Quaternion Algebra

    详细的进行了公式的推导。

    扩展卡尔曼滤波算法的关键部分就是对状态方程和观测方程进行线性化处理，然后对系统进行离散化处理，这里采用先将系统线性化，然后进行离散化，最后得出扩展卡尔曼滤波方程。

20. Inertial sensor technology trends → 2001

    MEMS drawbacks

21. Three-axis attitude determination from vector observations →2012

    Triad ，Wahba two nonparallel vector

22. Analytic steady-state accuracy solutions for two common spacecraft attitude estimators → 1978

    Gyro model

Quaternion Definitions (from ref.19)

q=[q0qv]=⎡⎣⎢⎢⎢⎢q0q1q2q3⎤⎦⎥⎥⎥⎥

A quaternion contains a rotation axis and a turn angle itself ,let u be the unit vector , θ be the angle or rotation.A quaternion could be represented with the following way:

q=⎡⎣⎢⎢⎢⎢⎢⎢cos(θ2)uxsinθ2)uysin(θ2)uzsinθ2)⎤⎦⎥⎥⎥⎥⎥⎥

The rotation matrix (Direction Cosine Matrix) can be derived by eq.(short of equation number)

Cnb=C(qbn)=(q20−qTvqv)I+2qvqTv+2q0⌊qv×⌋

the skew-symmetric matrix operator ⌊qv×⌋ is defined as:

⌊qv×⌋=⎡⎣⎢0q3−q2−q30q1q2−q10⎤⎦⎥

the quaternion derivative with time

q˙=12Ω(w)q=12Ξ(q)[0w]

w is the body angular rate which can be measured with a tri-aixs exteroceptive sensor gyroscope

w=⎡⎣⎢wxwywz⎤⎦⎥

Ω(w)=⎡⎣⎢⎢⎢⎢0wxwywz−wx0−wzwy−wywz0−wx−wz−wywx0⎤⎦⎥⎥⎥⎥=[0w−wT−⌊w×⌋]

Ξ(q)=⎡⎣⎢⎢⎢⎢−q1q0q3−q2−q2−q3q0q1−q3q2−q1q0⎤⎦⎥⎥⎥⎥=[−qvq0I3×3+⌊qv×⌋]

First Order Quaternion Integrator

q(tk+1)=(exp(12Ω(w¯¯¯)Δt)+148(Ω(w(tk+1)Ω(w(tk))−Ω(w(tk))Ω(w(tk+1)))Δt2)q(tk)

the w¯¯¯ here denotes w¯¯¯=w(tk+1)+w(tk)2

Gyro Noise Model:As part of the IMU, a three-axis gyro provides measurements of the rotational velocity,

Gyros are known to be subject to different error terms, such as a rate noise error and a bias.

use a simple but realistic model for gyro operation developed by Farrenkopf

wm=w+b+nr

The vector b is the drift-rate bias and br is the drift-rate noise. nr is assumed to be a Gaussian white-noise process

E[nr(t)]=0

E[nr(t)nTr(t′)]=Qr(t)δ(t−t′)

The drift-rate bias is itself not a static quantity but is driven by a second Gaussian white-noise process, the gyro drift-rate ramp noise.

gyro bias is non-static and simulated as a random walk process

b˙=nw

with

E[nw(t)]=0

E[nw(t)nTw(t′)]=Qw(t)δ(t−t′)

The state equation

x(t)=[q(t)b(t)]

the differential equations governing the state

q˙(t)=12Ω(wm−b−nr)q(t)

b˙=nw

the prediction equations

q^˙=12Ω(w^)q^(t)

b^˙=03×1

with

w^=wm−b^

the bias is constant over the integration interval.

---

System State Equation

{X˙(t)=f[X(t),t]+W(t)Z(t)=h[X(t),t]+V(t)

X(t+Δt)=X(t)+X˙(t)+12!(Δt)2+…=X(t)+f(X)Δt+δfδXf(X)(Δt)22!+…

A[X(k)]=δfδXf(X)

{X(k+1)=X(k)+f[X(k)]Δt+A[X(k)]f[X(k)](Δt)22!+W(k)Z(k)=h[X(k)]+V(k)

EKF procedure

• X^(k+1|k)=X^(k|k)+f[X^(k|k)]Δt+A[X^(k|k)]f[X^(k|k)](Δt)22!

• P(k+1|k)=Φ(k)P(k|k)+Qk

  Φ(k)=I+A[X^(k|k)]⋅Δt

• K(k+1)=P(k+1|k)HT(k+1)[H(k+1)P(k+1|k)HT(k+1)+Rk+1]−1

  H(k+1)=∂f∂X|X=X^(k+1|k)

• X^(k+1|k+1)=X^(k+1|k)+K(k+1)Z(k+1)−h[X^(k+1|k)]

• P(k+1|k+1)=[I−K(k+1)H(k+1)]P(k+1|k)

---

---

Singularity

Rnb=⎡⎣⎢cθcψcθsψ−sθcψsθsϕ−sψcϕsψsθsϕ+cψcϕsϕcθcψsθcϕ+sψsϕsψsθcϕ−cψsϕcϕcθ⎤⎦⎥

θ∈(−π2∼π2)ϕ,ψ∈[−π∼π]

θ=asin(−R31)

• if θ→π2sθ=1

R23−R12=2sψcϕ−2cψsϕ=2sin(ψ−ϕ)

R13+R22=2cψcϕ+2sψsϕ=2cos(ψ−ϕ)

ψ−ϕ=atanR23−R12R13+R22

• elseif
  
  θ→−π2sθ=−1

ψ−ϕ=atanR23+R12R13−R22

• else ϕ=atan(R32R33)

ψ=atan(R21R11)

θ∈[−π∼−π2)∪(π2∼π]

θ=−π⋅sign(R31)−asin(−R31)

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪ϕ=⎧⎩⎨atan(R32R33)π⋅sign(R32)+atan(R32R33)R33>0R33<0θ=asin(−R31)ψ=⎧⎩⎨atan(R21R11)π⋅sign(R21)+atan(R21R11)R11>0R11<0

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪ϕ=⎧⎩⎨atan(R32R33)−π⋅sign(R32)+atan(R32R33)R33>0R33<0θ=−π⋅sign(R31)−asin(−R31)ψ=⎧⎩⎨atan(R21R11)−π⋅sign(R21)+atan(R21R11)R11>0R11<0

now the singularity problem convert to the determine of ϕ

假定某一角度在进行大角度范围变动时，其它两个角则在小角度范围内变动，在该假定下完成了四元数到欧拉角的转换，显然这种考虑也是不完备的

---

By Fantasy

​

​

​

​

​