Extended Kalman Filter vs. Error State Kalman Filter for Aircraft Attitude Estimation

先大致介绍一下看这篇论文的动机，最近在面试时候提到自己用到了误差卡尔曼基本都会被问到误差卡尔曼和扩展卡尔曼的区别，但答的都不是非常理想，其中一个面试官推荐了这篇论文。

摘要

卡尔曼滤波器 (KF) 是当状态和测量动态本质上是线性时最小化均方误差的最佳估计器，前提是过程和测量噪声过程被建模为高斯白噪声。然而，在现实世界中，人们会遇到大量的场景，其中过程或测量模型（或两者）都是非线性的。在这种情况下，使用了一类称为扩展卡尔曼滤波器 (EKF) 的次优卡尔曼滤波器的实现。 EKF 的工作原理是将当前参考轨迹周围的非线性模型线性化，然后为线性化的模型设计卡尔曼滤波器增益。最近，针对某一类问题出现了一种替代方法，其中使用卡尔曼滤波器估计状态的误差，而不是状态本身。这种误差状态 KF (ErKF) 方法通过非线性模型的扰动推导误差状态动力学，有助于误差状态的最优更新以及误差状态协方差的最优预测和更新。这是因为误差状态动力学是线性的，从而满足最优卡尔曼滤波的条件。本文通过模拟提供了 EKF 和 ErKF 之间的比较，并表明 ErKF 性能对执行的各种机动的飞机具有鲁棒性。此外，本文表明，与 EKF 不同，ErKF 不需要针对噪声协方差反复调整以获得可接受的估计性能。

引言

所有状态变量均可以直接测量在实践中是罕见的。在物理系统中，状态向量的某些分量是无法访问的内部变量，它们要么无法测量，要么测量需要使用非常昂贵的测量设备。因此，测量所有状态变量要么不可行，要么非常昂贵。因此，在大多数实际场景中，确实需要通过已知测量来构建对未知状态变量的估计。

对于具有确定性扰动的线性动力系统，Luenberger 观测器为状态估计问题提供了一个完整而全面的答案[1]。在系统的过程和测量是线性的，并且被过程白噪声和测量白噪声扰动的系统，一些其历史可以追溯到1940年的早期的观测器设计方法，包括 Wiener-Hopf 滤波器，它是一个线性系统[2,3]和 Kalman-Bucy 滤波器[4-6]，这是一种基于模型的滤波方法。维纳滤波器对观测器理论的主要贡献在于使用谱分解技术推导了随机平稳标量过程的稳态最优滤波器和预测器。另一方面，在卡尔曼滤波理论的发展过程中，输入输出信号维纳模型被状态空间模型所取代，得到了平稳和非平稳情况下最优状态估计量的递推解。此外，卡尔曼滤波器在最小化系统状态误差协方差的迹的意义上提供了递归的解决方案。换句话说，卡尔曼滤波器也可以称为最小协方差估计器。维纳和卡尔曼滤波器是最小二乘意义上的最优滤波器，并且在稳态下彼此等效[7]。在上述滤波器设计中，假设维纳滤波器的情况下信号和噪声处理的频谱特性以及过程和测量的协方差矩阵是完全已知的。离散时间中典型的时不变线性系统表示为
ξk+1=Aξk+Buk+Gwk,ξ(0)=ξ0zk=Cξk+vk(1)\xi_{k+1} = A \xi_k + Bu_k + Gw_k, \xi(0) = \xi_0 \\ z_k = C\xi_k + v_k \tag{1} ξk+1=Aξk+Buk+Gwk,ξ(0)=ξ0zk=Cξk+vk(1)
其中ξk\xi_kξk表示系统状态向量，uku_kuk表示系统输入向量，wkw_kwk表示过程噪声向量，zkz_kzk表示系统输出向量，vkv_kvk表示测量噪声向量，AAA表示系统状态矩阵，BBB表示系统输入矩阵， GGG表示过程扰动矩阵，CCC表示输出/测量矩阵，ξ0\xi_0ξ0表示系统状态的初始值。使用卡尔曼滤波器 (KF) 的主要优点之一是它能够递推地预测和更新状态，使其成为在线应用的理想选择。应该注意的是，不需要测量系统的所有状态来估计它们。 KF 的原理是使用可用的测量来估计那些不一定需要测量的状态。然而，在过程动力学或测量或两者中具有固有非线性特性的系统下，线性KF的直接实现不能保证在最小化估计误差的均方根的意义上产生最优的结果。因此，对能够有效重构非线性系统未知状态的需求促进了非线性滤波理论的研究。

非线性滤波器的设计是一个非常具有挑战性的问题，并且在过去几十年的文献中受到了相当多的关注[8,9]。离散时间中典型的时不变非线性系统可以表示为
ξk+1=f(ξk,vk,wk),ξ(0)=ξ0zk=h(ξk)+vk(2)\xi_{k+1} = f(\xi_k,v_k,w_k), \xi(0) = \xi_0 \\ z_k = h(\xi_k) + v_k \tag{2} ξk+1=f(ξk,vk,wk),ξ(0)=ξ0zk=h(ξk)+vk(2)
其中，和以前一样，ξk\xi_kξk表示方程2中非线性系统的系统状态向量。 uku_kuk表示系统输入向量，wkw_kwk表示过程扰动向量，zkz_kzk表示系统输出向量，vkv_kvk表示测量扰动向量，f(⋅,⋅,⋅)f(·,·,·)f(⋅,⋅,⋅)表示非线性的过程函数，它将系统状态、输入和扰动映射到状态的微分（动力学），h(⋅)h(·)h(⋅)表示系统状态的非线性测量函数，ξ0\xi_0ξ0表示系统状态的初始值。过程和测量噪声过程具有零均值和规定的协方差，在文献中通常分别表示为QkQ_kQk和RkR_kRk。在开发非线性滤波器的众多尝试中，扩展卡尔曼滤波器 (EKF) 是最流行的方法。 EKF 的设计通常基于系统在每个时间步长沿着参考轨迹的一阶局部线性化[6,10-12]。因此，非线性系统动力学相对于系统状态的偏导数被计算并在状态状态下对于每一个时间步长进行评估。 EKF 是一个估计器，它处理动态过程和测量中的非线性，此外，类似于 KF，它处理过程和测量动态被高斯白噪声扰动的系统。 EKF[11,13-15]被概念化为对困难理论问题的工程近似。问题是关于在存在干扰的情况下从可用测量估计非线性系统的状态的问题。自被提出以来，EKF 已成功应用于非线性随机系统的状态估计[16-18]。对于在过程或测量中完全无噪声的系统，即完美模型和无噪声传感器， EKF 可以被设计为作为确定性非线性系统的状态估计器。[19]中的结果受到某些条件的测量的一些大偏差结果的强烈影响，如[20]中所报告的。[19]中的目标是描述为无噪声非线性动力系统构建观测器的设计过程，该观测器形成有噪声非线性动力系统的极限解。一个限制参数与过程以及测量噪声向量相关联，并且当这个限制参数接近 0 时，有噪声系统的观测器的行为接近于无噪声确定性系统的观测器的行为。十多年来，EKF 的稳定性和收敛特性一直是研究的主题。在[21]表明 EKF 保证是一个非发散的估计器。当估计状态替换为稳定状态反馈中的实际状态时，非发散估计器可用于创建基于模型的非线性控制系统而不会失去稳定性。保证 EKF 不发散的条件大致是非线性有界斜率，输入是加性输入，系统是 M-可检测的。在[19]中报告的研究中，通过有效构建确定性观测器来达到随机滤波器的渐近极限。文献研究表明，虽然EKF对一般类型的系统表现出指数收敛行为，但这些结果在本质上仍然是局部的[22-24,26,27]。在[22]中展示了合适的QkQ_kQk和RkR_kRk的选择与EKF的有效性之间的连接。在[23]中开发了一种线性化技术，该技术包括引入未知对角矩阵以将近似误差考虑在内。显示稳定性结果的问题被重新转换为线性矩阵不等式问题，这反过来建立了 EKF的良好收敛行为与协方差矩阵QkQ_kQk和RkR_kRk之间的联系。在[26]中，EKF被设计为具有先验规定的稳定度。EKF误差动力学的平衡点被证明是一个指数稳定的平衡点。在[26]建立结果的一个关键假设是高阶泰勒级数项的范数是成比例的误差项的平方的上限。这个假设有助于说明EKF是一个指数观测器。此外，在[22]和[26]中，已经详细研究了 EKF的吸引域。一种建议是将QkQ_kQk矩阵设置得足够大，以便EKF在状态估计误差的初始化中可以容忍任意大的误差。在[24]中作者建立了连续-连续（过程和测量是连续的）和连续-离散（过程是连续的，而测量是离散的）EKF的稳定性结果，这些EKF是从[28]的观测器中导出的。[24]的作者表明，只要噪声协方差矩阵 (QkQ_kQk和RkR_kRk) 被适当地参数化，EKF就会采用随增益增加而渐近地接近固定增益观测器的时变高增益观测器的形式。此外，在[24]中表明，在测量的 Lie导数形成系统非线性的微分同胚和全局利普西兹性质的条件下，EKF是一类可转换为下三角形式的非线性系统的全局指数观测器。在[29]中，提出了一种用于聚合反应器标准非线性模型的基于观测器的控制结构，使用经典的输入/输出线性化技术进行控制器设计并保证全局渐近闭环稳定性。通过指数收敛观测器估计未知状态，类似于EKF的方程，用于形成控制律。在[30]中表明，对估计误差的非线性和指数稳定性的限制性假设，并不能保证闭环系统的行为，即使系统在状态反馈下是指数稳定的。因此，已经进行了大量的研究工作来分析闭环系统的行为，并且使用诸如EKF之类的估计器作为观测器。为此[18]放宽了全局李普西兹条件，并考虑了一类可转换为具有线性内部动力学的特殊正规形式的系统。带有 EKF 的闭环系统然后通过黎卡迪方程的参数化以标准奇异点扰动形式表示。此外，通过放宽全局李普西兹条件，峰值现象[33]的结果可能会导致困难，这可能导致闭环系统的不稳定。当使用高增益反馈产生具有非常负实部的特征值时，会发生峰化现象，其中一些状态在快速衰减到零之前达到峰值。这种现象通常通过将控制全局限制在感兴趣的紧凑区域之外来克服。 EKF 的许多成功应用在[32,34]和其他领域中有所描述，例如膜生物反应器系统的监测[35-37]、自适应状态估计、非线性估计[38]、参数估计、目标跟踪、神经网络训练、编队飞行[39,40]、弹道弹丸状态估计[41-43]、未知地形环境中的导航[44,45]等。

工程师可以使用数值近似来解决精确问题，或者他们可以精确地解决近似问题” - Fred Daum[9]。将实际问题中出现的非线性动力学和/或非线性测量方程线性化并基于线性化方程计算卡尔曼增益的过程是EKF的原理。相比之下，[46,47]中描述的误差状态卡尔曼滤波器通过将问题从状态域重新转换为误差状态域来解决具有精确解的近似问题。

误差状态卡尔曼滤波器 (ErKF) 是在[46]中针对移动机器人定位问题引入的。在这个问题中，以及在一般的自主导航问题中，位置和姿态的准确估计对于制导和控制算法有效执行至关重要。陀螺仪、加速度计和磁力计等内部传感器（安装在本机上）分别用于提供瞬时角速度、加速度和航向的测量[47,48]。外部传感器，例如全球定位系统 (GPS) 提供位置、速度和车辆的航迹角。很多时候，各种传感器的测量结果会融合在一起，以提高估计的质量[47,48]。然而，最基本的导航形式依赖于内部安装的传感器或所谓的“捷联”惯性传感器[48]。惯性导航系统 (INS) 被广泛使用用于飞行器导航和户外机器人中[46,49]。构成 INS 的传感器套件称为惯性测量单元 (IMU)，它通常由测量角速度的陀螺仪和测量机身轴上比力的加速度计组成。INS 通过航位推算来计算方向、位置和速度的估计值。它通过分别使用已知的初始方向和速度对角速度和加速度的刚体运动学方程进行积分以获得方向（姿态）估计和速度估计，并在已知初始位置的情况下通过对速度方程进行积分以获得位置估计来实现这一点。

众所周知，惯性传感器存在零偏以及需要建模的随机误差[47]。经典 INS 集成了 IMU 中传感器提供的测量值，以获得姿态以及位置和速度估计。因此，如果不定期重置或不使用外部测量的帮助，姿态、位置和速度计算中的误差会随着时间无限增长。基于卡尔曼滤波器的算法已用于估计绝对状态，例如姿态、位置和速度，以及偏差和其他误差参数。对于姿态估计问题，使用了两类（过程）建模方式：动态或车辆特定建模和捷联建模。以欧拉角表示的姿态运动学由 [48,50,51]给出
[ψ˙θ˙ϕ˙]=1cosθ[0sinϕcosϕ0cosϕcosθ−sinϕcosθcosθsinϕsinθcosϕsinθ][pqr](3)\begin{bmatrix} \dot{\psi} \\ \dot{\theta} \\ \dot{\phi} \end{bmatrix} = \frac{1}{cos\theta} \begin{bmatrix} 0 & sin\phi & cos\phi \\ 0 & cos\phi cos \theta & -sin \phi cos \theta \\ cos \theta & sin \phi sin\theta & cos \phi sin \theta \end{bmatrix} \begin{bmatrix} p \\ q \\ r \end{bmatrix} \tag{3} ⎣⎡ψ˙θ˙ϕ˙⎦⎤=cosθ1⎣⎡00cosθsinϕcosϕcosθsinϕsinθcosϕ−sinϕcosθcosϕsinθ⎦⎤⎣⎡pqr⎦⎤(3)

其中ψ\psiψ、θ\thetaθ和ϕ\phiϕ是偏航角、俯仰角和滚转角，ppp、qqq和rrr是角速度或速率陀螺仪测量的速率。如果初始姿态已知，姿态可以通过方程（3）的积分获得。在[52]中，采用所谓的动态建模方法，其中角速率包含在要估计的状态向量中，而由陀螺仪测量的速率用作 KF 的测量值。在捷联模型中，如[46-48,50,51]所采用的那样，状态向量中包含姿态中的误差而不是姿态本身，而辅助传感器测量的姿态用作测量值。然而，所谓的动态建模方法（在[46]中也被称为直接过滤方法）有以下缺点[46,51]：(a) 动态模型依赖于车辆，因此每次进行修改，或部分被重新定位，或其质量发生变化等，动态建模必须改变，(b) 动态建模需要更多的状态和车辆运动参数的知识，以及 © 对相互作用的精确建模需要平台和环境。在[46,51]中，一种误差状态滤波器被推导出，其中姿态误差包含在状态向量中，而误差传播方程是从包含陀螺仪测量的方程（3）中给出的捷联方程推导出来的。在误差状态公式中，欧拉角是通过对方程（3）中给出的设备进行积分获得的，而估计的误差在每个时间步都被反馈到欧拉角中。陀螺仪测量中的零偏也可以包含在状态向量中，并且可以在每个时间步反馈给设备。 KF中用于误差状态公式的测量值是通过其他辅助传感器（例如加速度计和磁力计）获得的姿态测量值。 ErKF 的一些成功应用在[47,50,71]中进行了描述。在[71]中报道的研究中，使用视觉传感器开发了同步定位和建图 (SLAM) 算法，用于在未知的城市环境中进行导航，其中全球定位系统 (GPS) 中断是一种常见现象，或者GPS信号不可用。

EKF 的固有限制之一是重新调整噪声协方差矩阵QkQ_kQk和RkR_kRk和初始状态误差协方差矩阵，以适应车辆运行的特定情况[53-55]。众所周知，这些协方差矩阵的不确定性对 EKF 的性能有影响。由于调整不当，这些协方差矩阵中的任何不确定性都会对滤波器产生不利影响，有时甚至可能导致其发散。在传统设置中，噪声协方差矩阵的确定需要对过程和测量模型有很好的了解。在本文中，我们建议在不重新调整噪声和状态误差协方差矩阵的情况下，比较 EKF 和 ErKF 在一组飞机机动中的性能。因此，一旦这些协方差矩阵已经根据过程和传感器规格进行了调整，当遇到不同的飞行条件（例如存在或不存在湍流）或执行不同的飞机机动时，协方差矩阵不一定是最佳的，就不应该重新调整它们。据我们所知，将 ErKF 与传统 EKF 进行这种滤波器性能比较还没有被尝试过。此外，本文试图通过问题表述和滤波器设计指出 EKF 和 ErKF 之间的一些差异。论文的其余部分结构如下：第 II 部分提供了所考虑问题的数学公式以及设计 EKF 和 ErKF 的框架。在第三节中，一个典型的姿态航向参考系统问题被公式化用于飞机姿态估计，其测量来自传感器，如陀螺仪和加速度计。第四节针对第三节中针对一系列不同飞机机动开发的问题提供了一些模拟。此外，我们在第四节中提出了一个重要的评论（Remark IV.1），它构成了本文的基础。对于所有这些机动，EKF 和 ErKF 的性能被进行了比较和对比。最后，第五节包含一些总结性评论和一些未来计划。在整篇论文中，粗体表示向量，大写字母表示矩阵，小写字母表示标量。

通用问题公式

考虑具有连续过程动力学和离散测量的非线性、有界、可观测系统为
x˙(t)=f(x(t),u(t))+Γw(t),x(0)=x0zk=h(xk)+vk(4)\dot{x}(t) = f(x(t),u(t)) + \Gamma w(t), x(0) = x_0 \\ z_k = h(x_k) + v_k \tag{4} x˙(t)=f(x(t),u(t))+Γw(t),x(0)=x0zk=h(xk)+vk(4)
其中，x∈Dx⊂Rnx \in \mathcal{D}_x \subset \mathcal{R}^nx∈Dx⊂Rn表示系统的nnn−维状态向量，u∈Du⊂Rmu \in \mathcal{D}_u \subset \mathcal{R}^mu∈Du⊂Rm表示mmm−维已知输入向量，w∈Dw⊂Rnww \in \mathcal{D}_w \subset \mathcal{R}^{n_w}w∈Dw⊂Rnw表示nwn_wnw−维随机过程噪声，zk∈Dz⊂Rpz_k \in \mathcal{D}_z \subset \mathcal{R}^pzk∈Dz⊂Rp表示ppp-维系统测量，vk∈Dv⊂Rpv_k \in \mathcal{D}_v \subset \mathcal{R}^pvk∈Dv⊂Rp表示ppp-维随机测量噪声，f(⋅,⋅)：(Dx⊂Rn)×(Du⊂Rm)f(\cdot,\cdot)：(\mathcal{D}_x \subset \mathcal{R}^n) \times (\mathcal{D}_u \subset \mathcal{R}^m)f(⋅,⋅)：(Dx⊂Rn)×(Du⊂Rm)是系统状态的有限非线性映射，h(⋅)：Dx⊂Rn→Rph(\cdot)：\mathcal{D}_x \subset \mathcal{R}^n \to \mathcal{R}^ph(⋅)：Dx⊂Rn→Rp是系统状态的非线性映射，Γ∈Rn×nw\Gamma \in \mathcal{R}^{n \times n_w}Γ∈Rn×nw是过程噪声缩放矩阵。过程和测量噪声被假定为零均值、带宽限制、不相关、多元高斯白噪声过程，由下式给出
E[w(t)wT(τ)]=Qδ(t−τ)={Q,t=τ0,t≠τE[vkvjT]=Rkδkj={Rk,k=j0,k≠j(5)E[w(t)w^T(\tau)] = Q \delta(t - \tau) = \begin{cases} Q, t = \tau \\ 0, t \neq \tau \end{cases} \\ E[v_kv_j^T] = R_k\delta_{kj} = \begin{cases} R_k , k = j \\ 0, k \neq j \end{cases} \tag{5} E[w(t)wT(τ)]=Qδ(t−τ)={Q,t=τ0,t=τE[vkvjT]=Rkδkj={Rk,k=j0,k=j(5)
其中，QQQ是连续过程噪声协方差，RkR_kRk是离散测量噪声协方差，δ(⋅)\delta(·)δ(⋅)是Dirac delta函数，δkj\delta_{kj}δkj是Kronecker delta函数，kkk是离散下标。随机变量www和vkv_kvk通常分别表示为w∼N(0,Q)w \sim \mathcal{N}(0,Q)w∼N(0,Q)和vk∼N(0,Rk)v_k \sim \mathcal{N}(0,R_k)vk∼N(0,Rk)，其中www和vkv_kvk000均并且协方差为QQQ和RkR_kRk的分布。方程（4）中系统的初始状态被假定为一个高斯随机向量，其均值为xˉ0\bar{x}_0xˉ0，协方差为Pˉ0\bar{P}_0Pˉ0，可以表示为x0∼N(xˉ0,Pˉ0)x_0 \sim \mathcal{N}(\bar{x}_0, \bar{P}_0)x0∼N(xˉ0,Pˉ0)。

备注2.1 对于本文提出的方法，可以假设输入u(t)u(t)u(t)为0，这涵盖了开环估计问题的情况，或者可以使用状态向量的真值来形成u(t)u(t)u(t)，这解决了闭环估计的情况。在任何一种情况下，将要解决的主要问题是评估估计器的性能。

备注2.2 向量参数xxx的向量值函数fff在操作点αˉ\bar{\alpha}αˉ的泰勒级数展开式f(⋅)：x∈Rn→Rnf(\cdot)：x \in \mathcal{R}^n \to \mathcal{R}^nf(⋅)：x∈Rn→Rn由下式给出
f(x)=f(αˉ)+▽f(x)∣x=αˉ(x−αˉ)+12!▽2f(x)∣x=αˉ(x−αˉ)+HOT(6)f(x) = f(\bar{\alpha}) + \triangledown f(x)|_{x = \bar{\alpha}}(x - \bar{\alpha}) + \frac{1}{2!} \triangledown^2f(x) |_{x = \bar{\alpha}}(x-\bar{\alpha}) + HOT \tag{6} f(x)=f(αˉ)+▽f(x)∣x=αˉ(x−αˉ)+2!1▽2f(x)∣x=αˉ(x−αˉ)+HOT(6)
其中，算子▽f(x)\triangledown f(x)▽f(x)，也称为梯度，表示在操作点αˉ\bar{\alpha}αˉ处对xxx求值的一阶偏微分，▽2f(x)\triangledown^2f(x)▽2f(x)表示关于xxx的海塞矩阵，HOTHOTHOT表示高阶项。

备注2.3 考虑非线性系统
x˙(t)=f(x(t))(7)\dot{x}(t) = f(x(t)) \tag{7} x˙(t)=f(x(t))(7)
其中x∈Rnx \in \mathcal{R}^nx∈Rn是待估计的状态向量，f(⋅)f(\cdot)f(⋅)表示连续时间非线性动力学，它是一个足够平滑的函数。然后方程。 (7) 可以通过x(t+Δt)x(t + \Delta t)x(t+Δt)的欧拉级数展开式进行离散化[41]，即
x(t+Δt)=x(t)+x˙(t)Δt+x¨(t)(Δt)2+HOT(8)x(t+\Delta t) = x(t) + \dot{x}(t)\Delta t + \ddot{x}(t)(\Delta t)^2 + HOT \tag{8} x(t+Δt)=x(t)+x˙(t)Δt+x¨(t)(Δt)2+HOT(8)
其中，̇x˙(t)=dxdt\dot{x}(t) = \frac{dx}{dt}x˙(t)=dtdx，x¨(t)=d2xdt2\ddot{x}(t) = \frac{d^2x}{dt^2}x¨(t)=dt2d2x，Δt\Delta tΔt表示小时间步长，HOTHOTHOT表示高阶项。一阶欧拉展开式如下
x(t+Δt)=x(t)+x˙(t)Δt⇒x˙(t)=x(t+Δt)−x(t)Δt(9)x(t + \Delta t) = x(t) + \dot{x}(t)\Delta t \\ \Rightarrow \dot{x}(t) = \frac{x(t + \Delta t) - x(t)}{\Delta t} \tag{9} x(t+Δt)=x(t)+x˙(t)Δt⇒x˙(t)=Δtx(t+Δt)−x(t)(9)

A 扩展卡尔曼滤波器

扩展卡尔曼滤波器 (EKF) 的目标是通过使用方程（4）中给出的给出的zkz_kzk的可用测量集获得由方程（4）中给出的系统状态向量x(t)x(t)x(t)的估计。

1. EKF滤波器初始化

EKF 模拟从 EKF 状态估计值x^(t)\hat{x}(t)x^(t)和状态误差协方差矩阵P(t)P(t)P(t)在t=0t=0t=0的初始化开始。这些在以下等式中给出：
x^(0)=x^0=E[x(0⋅Δt)]=E[x0]P(0)=P0=E[(x0−x^0)(x0−x^0)T](10)\hat{x}(0) = \hat{x}_0 = E[x(0\cdot \Delta t)] = E[x_0] \\ P(0) = P_0 = E[(x_0 - \hat{x}_0)(x_0 - \hat{x}_0)^T] \tag{10} x^(0)=x^0=E[x(0⋅Δt)]=E[x0]P(0)=P0=E[(x0−x^0)(x0−x^0)T](10)
其中，x^0\hat{x}_0x^0表示初始 EKF 状态估计，P0P_0P0表示 EKF 初始状态误差协方差矩阵，x0x_0x0表示模型初始状态，Δt\Delta tΔt表示以秒为单位的采样时间。

备注2.4 扩展卡尔曼滤波器 (EKF) 的设计分两个阶段完成，分别是

1.  预测阶段
2.  更新阶段

在预测阶段，滤波器基于先前对状态向量的估计，通过等式（4）中的非线性映射来预测状态向量的估计。在更新阶段，过滤器使用当前时刻的可用测量来校正在预测阶段获得的状态的预测估计。

备注2.5 由于过程模型由方程（4）的第一个方程给出。方程（4）的第一个方程是连续时间下给出的，方程（4）中的测量方程是离散时间下给出的。通过对输入进行欧拉积分导出的在时间ttt的连续时间状态估计预测x^(t)\hat{x}(t)x^(t)和状态误差协方差矩阵预测P(t)P(t)P(t)分别表示为x^k−\hat{x}_k^{-}x^k−和Pk−P_k^{-}Pk−。此外，在离散时间kkk的更新阶段之后，更新的状态估计x^k\hat{x}_kx^k和更新的状态误差协方差估计PkP_kPk形成预测方程在t=t+Δtt = t + \Delta tt=t+Δt的输入。

2. EKF预测方程

在 EKF 的预测阶段，EKF 在ttt时刻的状态向量通过由f(x(t),u(t))f(x(t),u(t))f(x(t),u(t))给出的非线性映射基于 EKF 在t−Δtt- \Delta tt−Δt时刻的估计进行预测。因此，EKF 的预测方程如下
x^˙(t)=f(x^(t),u(t))P˙(t)=F(t)P(t)+P(t)FT(t)+ΓQΓTF(t)=∂f(x(t),u(t))∂x(t)∣x(t)=x^(t),u(t)(11)\dot{\hat{x}}(t) = f(\hat{x}(t),u(t)) \\ \dot{P}(t) = F(t)P(t) + P(t)F^T(t) + \Gamma Q \Gamma^T \\ F(t) = \frac{\partial f(x(t),u(t))}{\partial x(t)}|_{x(t) = \hat{x}(t),u(t)} \tag{11} x^˙(t)=f(x^(t),u(t))P˙(t)=F(t)P(t)+P(t)FT(t)+ΓQΓTF(t)=∂x(t)∂f(x(t),u(t))∣x(t)=x^(t),u(t)(11)

其中，x^k=x^(kΔt)\hat{x}_k = \hat{x}(k \Delta t)x^k=x^(kΔt)是kkk时刻的状态估计，Pk=P(kΔt)P_k = P(k \Delta t)Pk=P(kΔt)是kkk时刻的状态误差协方差估计，F(t)F(t)F(t)是状态非线性映射相对于状态在 EKF 状态估计值处在时刻ttt评估的的偏导数。QQQ是由方程（5）给出的连续过程噪声协方差矩阵。

3. EKF过渡到更新阶段

由于这里考虑的问题公式是针对连续过程和离散测量的，因此预测方程在连续时间内，而更新方程需要在离散时间内。因此，在对状态估计和状态误差协方差估计在时刻ttt进行了预测后，对状态估计和状态误差协方差估计的校正是通过对x^(t)\hat{x}(t)x^(t)和P(t)P(t)P(t)进行采样来进行的
x^k−=x^(kΔt)+x^˙⋅ΔtPk−=P(kΔt)+P˙(t)⋅Δt(12)\hat{x}_{k}^{-} = \hat{x}(k \Delta t) + \dot{\hat{x}} \cdot \Delta t \\ P_k^{-} = P(k \Delta t) + \dot{P}(t) \cdot \Delta t \tag{12} x^k−=x^(kΔt)+x^˙⋅ΔtPk−=P(kΔt)+P˙(t)⋅Δt(12)
其中，x^k−\hat{x}_k^{-}x^k−是kkk时刻离散化的预测状态估计，Pk−P_k^{-}Pk−是kkk时刻离散化的预测状态误差协方差。

4. EKF更新方程

EKF 设计的最后阶段是更新阶段。 EKF 预测的更新或校正是通过可用的测量来执行的。方程如下
Kk=Pk−HkT(HkPk−HkT+Rk)−1x^k=x^k−+Kk(zk−z^k−),z^k−=h(x^k−)Pk=(I−KkHk)Pk−Hk=∂h(xk)∂xk∣xk=x^k−(13)K_k = P_k^{-}H_k^{T}(H_kP_k^{-}H_k^T + R_k)^{-1} \\ \hat{x}_k = \hat{x}_k^{-} + K_k(z_k - \hat{z}_k^{-}), \hat{z}_k^{-} = h(\hat{x}_k^{-}) \\ P_k = (I - K_k H_k)P_k^{-} \\ H_k = \frac{\partial h(x_k)}{\partial x_k}|_{x_k = \hat{x}_k^{-}} \tag{13} Kk=Pk−HkT(HkPk−HkT+Rk)−1x^k=x^k−+Kk(zk−z^k−),z^k−=h(x^k−)Pk=(I−KkHk)Pk−Hk=∂xk∂h(xk)∣xk=x^k−(13)
其中，x^k\hat{x}_kx^k是kkk时刻基于kkk时刻由zkz_kzk给出的可用测量的校正/更新后的状态估计，PkP_kPk是kkk时刻校正/更新后的状态误差协方差矩阵，HkH_kHk是线性化后的测量矩阵，RkR_kRk是方程（5）给出的测量噪声协方差矩阵。 KkK_kKk是通过最小化均方状态误差获得的卡尔曼增益矩阵[6]。这种最小化可以看作是最小化状态误差协方差矩阵Pk=E[ekekT]P_k = E[e_ke_k^T]Pk=E[ekekT]的迹，其中ek=xk−x^ke_k = x_k - \hat{x}_kek=xk−x^k。

B. 误差状态卡尔曼滤波器（ErKF）

考虑由方程（4）的第一个方程给出的连续时域中的非线性状态空间过程模型。其中，如前所述，xxx是状态向量，uuu是控制输入，www是过程噪声向量，它被建模为有限带宽高斯白噪声过程，具有指定的协方差QQQ，由等式（5）给出。为了推导误差状态过程模型，我们忽略输入和过程噪声并考虑以下简化的非线性过程模型
x˙(t)=f(x(t))(14)\dot{x}(t) = f(x(t)) \tag{14} x˙(t)=f(x(t))(14)
其中，f(x)=[f1,f2,...,fn]T∈Rnf(x) =[f_1,f_2,... ,f_n]^T \in \mathcal{R}^nf(x)=[f1,f2,...,fn]T∈Rn。在x(t)x(t)x(t)附近应用一个小的扰动δx(t)\delta x(t)δx(t)
x˙+δx˙(t)=f(x(t)+δx(t))(15)\dot{x} + \delta \dot{x}(t) = f(x(t) + \delta x(t)) \tag{15} x˙+δx˙(t)=f(x(t)+δx(t))(15)
将关于xxx的泰勒级数展开应用到右手边 (RHS) ，并为了符号方便而忽略时间符号ttt
f(x(t)+δx(t))=f(x(t))+▽f(x(t))(x(t)+δx(t)−x(t))+O(t,x(t),δx(t))=f(x(t))+▽f(x(t))δx(t)+O(t,x(t),δx(t))(16)f(x(t) + \delta x(t)) = f(x(t)) + \triangledown f(x(t))(x(t) + \delta x(t) - x(t)) + \mathcal{O}(t,x(t),\delta x(t)) \\ = f(x(t)) + \triangledown f(x(t)) \delta x(t) + \mathcal{O}(t,x(t),\delta x(t)) \tag{16} f(x(t)+δx(t))=f(x(t))+▽f(x(t))(x(t)+δx(t)−x(t))+O(t,x(t),δx(t))=f(x(t))+▽f(x(t))δx(t)+O(t,x(t),δx(t))(16)
其中O\mathcal{O}O代表高阶项，▽f(x)=[(∂f1∂x)T(∂f2∂x)T⋯(∂fn∂x)T]T\triangledown f(x) = \begin{bmatrix}(\frac{\partial f_1}{\partial x})^T & (\frac{\partial f_2}{\partial x})^T & \cdots & (\frac{\partial f_n}{\partial x})^T\end{bmatrix}^T▽f(x)=[(∂x∂f1)T(∂x∂f2)T⋯(∂x∂fn)T]T表示f(x)f(x)f(x)的梯度（一阶偏导数）。假设扰动很小，即δx\delta xδx很小，使得δx2≈0\delta x^2 \approx 0δx2≈0，将方程（16）代入方程（15）
x˙(t)+δx˙(t)=f(x(t))+▽f(x(t))δx(t)+O(t,x(t),δx(t))(17)\dot{x}(t) + \delta \dot{x}(t) = f(x(t)) + \triangledown f(x(t)) \delta x(t) + \mathcal{O}(t,x(t),\delta x(t)) \tag{17} x˙(t)+δx˙(t)=f(x(t))+▽f(x(t))δx(t)+O(t,x(t),δx(t))(17)
此外，将方程（14）代入方程（17）中。忽略高阶项产生以下以δx\delta xδx为状态向量的线性时变系统。
δx˙(t)=F(t)δx(t)(18)\delta \dot{x}(t) = F(t)\delta x(t) \tag{18} δx˙(t)=F(t)δx(t)(18)
其中，F(t)=▽f(x(t))F(t) = \triangledown f(x(t))F(t)=▽f(x(t))。进一步注意，矩阵FFF与误差向量δx\delta xδx无关，从而导致误差状态传播的线性模型。离散化方程(18) 可得
δx(t+Δt)−δx(t)Δt=F(t)δx(t)(19)\frac{\delta x(t + \Delta t) - \delta x(t)}{\Delta t} = F(t) \delta x(t) \tag{19} Δtδx(t+Δt)−δx(t)=F(t)δx(t)(19)
其中，δx˙(t)\delta \dot{x}(t)δx˙(t)是通过一阶欧拉级数展开来近似的，如等式（9）所示。进一步简化方程（19）可得
δx(t+Δt)=δx(t)+F(t)δx(t)Δt=(I+F(t)Δt)δx(t)(20)\delta x(t + \Delta t) = \delta x(t) + F(t) \delta x(t) \Delta t \\ = (I + F(t) \Delta t)\delta x(t) \tag{20} δx(t+Δt)=δx(t)+F(t)δx(t)Δt=(I+F(t)Δt)δx(t)(20)
定义δx(t+Δt)=δxk+1\delta x(t + \Delta t) = \delta x_{k+1}δx(t+Δt)=δxk+1，F(t)=F(k)F(t) = F(k)F(t)=F(k)以及(I+FkΔt)=Φk(I + F_k \Delta t) = \Phi_k(I+FkΔt)=Φk, 方程（20）进一步简化为
δxk+1=Φkδxk(21)\delta x_{k+1} = \Phi_k \delta x_k \tag{21} δxk+1=Φkδxk(21)
其中Φk\Phi_kΦk是状态转移矩阵，Δt\Delta tΔt 是离散化时间间隔。

1. ErKF 运算方程

回想一下，精确连续时间动力学被给定为
x˙(t)=f(x(t))(22)\dot{x}(t) = f(x(t)) \tag{22} x˙(t)=f(x(t))(22)
其中x∈Rnx \in \mathcal{R}^nx∈Rn是待估计的状态向量，f(⋅)∈Rnf(\cdot) \in \mathcal{R}^nf(⋅)∈Rn表示至少一次可微的连续时间非线性动力学。假设状态x(t)∣t=0x(t)|_{t = 0}x(t)∣t=0的初始条件是已知的，方程（22）可以在时间间隔(0,Δt)(0,\Delta t)(0,Δt)上积分得到
x(t)=∫0Δtf(x(t))dt(23)x(t) = \int_{0}^{\Delta t}{f(x(t))}dt \tag{23} x(t)=∫0Δtf(x(t))dt(23)
在Δt\Delta tΔt的周期内对连续时间函数x(t)x(t)x(t)进行采样，产生方程（23）的离散化版本。即x(t)∣t=kΔtx(t)|_{t = k \Delta t}x(t)∣t=kΔt，其中k=0,1,...,Kk = 0,1,...,Kk=0,1,...,K。在[50,51]中提出的误差状态滤波器设计中，预测状态被认为是在（23）中进行的动力学积分结果的离散化版本。
x^k−=x(t)∣t=kΔt(24)\hat{x}_k^{-} = x(t)|_{t = k\Delta t} \tag{24} x^k−=x(t)∣t=kΔt(24)
其中，x^k−\hat{x}_k^{-}x^k−是从（23）中获得的kkk时刻得预测估计值x(t)∣t=kΔtx(t)|_{t = k \Delta t}x(t)∣t=kΔt。此外，考虑构成卡尔曼滤波器基础的连续时间状态空间模型。在方程（18）中获得的（线性化的）误差状态过程模型，由过程噪声增强后由下式给出
δx˙(t)=F(t)δx(t)+Γw(t)(25)\delta \dot{x}(t) = F(t)\delta x(t) + \Gamma w(t) \tag{25} δx˙(t)=F(t)δx(t)+Γw(t)(25)
其中δx(t)∈Rn\delta x(t) \in \mathcal{R}^nδx(t)∈Rn是误差状态（状态x(t)x(t)x(t)得误差），F(x(t))∈Rn×nF(x(t)) \in \mathcal{R}^{n \times n}F(x(t))∈Rn×n是系统矩阵，w∈Rnww \in \mathcal{R}^{n_w}w∈Rnw是过程噪声向量，Γ∈Rn×nw\Gamma \in \mathcal{R}^{n \times n_w}Γ∈Rn×nw是过程噪声增益矩阵。从方程（25）可以看出，误差状态过程模型是线性的。类似于（18）离散化后得到的方程，方程（25）中的连续时间误差状态过程模型被离散化后得到
δxk=Φk−1δxk−1+w~k−1(26)\delta x_k = \Phi_{k-1} \delta x_{k-1} + \tilde{w}_{k-1} \tag{26} δxk=Φk−1δxk−1+w~k−1(26)
其中w~k−1=∫0ΔtΦ(t,0)Γw(t)\tilde{w}_{k-1} = \int_{0}^{\Delta t}{\Phi(t,0) \Gamma w(t)}w~k−1=∫0ΔtΦ(t,0)Γw(t)是离散化过程噪声，Φk−1\Phi_{k-1}Φk−1是状态转移矩阵[57]。假设测量模型处于直接离散的时间
zk=h(xk)+vk(27)z_k = h(x_k) + v_k \tag{27} zk=h(xk)+vk(27)
其中z∈Rlz \in \mathcal{R}^lz∈Rl是lll-维测量向量，h(⋅)h(\cdot)h(⋅)是公式（4）中定义的非线性测量映射，另外v∼N(0,Rk)v \sim \mathcal{N}(0,R_k)v∼N(0,Rk)是假设为零均值高斯白噪声（与过程噪声www不相关）的测量噪声，协方差表示为RkR_kRk，如方程（5）所示。误差状态预测被建模为 0，如[50,51]中所述。这具有在每个时刻重置预测误差的效果，因为在 ErKF 实现中，关注的是瞬时误差而不是误差的历史。换句话说，由δx^k−\delta \hat{x}_k^{-}δx^k−表示的 ErKF 的预测误差由小扰动组成，并被建模为零均值过程，条件是获得并包括时间瞬间k−1k-1k−1的所有测量值。假设初始状态x0∣0x_{0|0}x0∣0、初始误差状态δx0∣0\delta_{x0|0}δx0∣0和初始误差状态协方差P0∣0P_{0|0}P0∣0已知，误差状态协方差矩阵传播使用
Pk−=Φk−1Pk−1Φk−1T+Qk−1(28)P_k^{-} = \Phi_{k-1}P^{k-1}\Phi_{k-1}^T + Q_{k-1} \tag{28} Pk−=Φk−1Pk−1Φk−1T+Qk−1(28)
其中Qk−1Q_{k-1}Qk−1是离散时间等效过程噪声协方差矩阵，由下式给出
Qk−1=∫0ΔtΦ(t,0)ΓQΓTΦT(t,0)dt(29)Q_{k-1} = \int_{0}^{\Delta t}{\Phi(t,0) \Gamma Q \Gamma^T \Phi^T(t,0)dt} \tag{29} Qk−1=∫0ΔtΦ(t,0)ΓQΓTΦT(t,0)dt(29)
其中，Φ(t,0)=eF(t)t\Phi(t,0) = e^{F(t)t}Φ(t,0)=eF(t)t是连续时间状态转移矩阵，QQQ是方程（5）给出的连续时间过程协方差矩阵。创新过程，用Ik\mathcal{I}_kIk表示为
Ik=zk−h(x^k−)(30)\mathcal{I}_k = z_k - h(\hat{x}_k^{-}) \tag{30} Ik=zk−h(x^k−)(30)
其中x^k−\hat{x}_k^{-}x^k−通过方程（23）和（24）获得。值得注意的是，通过对方程（3）中的精确动力学进行积分和离散化获得的预测状态被前馈到 ErKF 中。创新协方差SkS_kSk和卡尔曼滤波器增益KkK_kKk可以计算如下[6]
Sk=Rk+HkPk−HkTKk=Pk−HkTSk−1(31)S_k = R_k + H_k P_k^{-}H_k^T \\ K_k = P_k^{-}H_{k}^TS_k^{-1} \tag{31} Sk=Rk+HkPk−HkTKk=Pk−HkTSk−1(31)
其中HkH_kHk是公式（13）中定义的雅各比。而更新的误差状态估计和更新的状态估计计算为
δx^k=KkIkx^k=x^k−+δx^k(32)\delta \hat{x}_k = K_k \mathcal{I}_k \\ \hat{x}_k = \hat{x}_k^{-} + \delta \hat{x}_k \tag{32} δx^k=KkIkx^k=x^k−+δx^k(32)
最后，更新后的误差状态协方差矩阵计算为
Pk=(I−KkHk)Pk−(33)P_k = (I-K_k H_k)P_k^{-} \tag{33} Pk=(I−KkHk)Pk−(33)
请注意，更新的误差状态估计δx^k\delta \hat{x}_kδx^k不包含预测的误差状态估计δx^k−\delta \hat{x}_k^{-}δx^k−在标准 ErKF 实现的情况下[50,51]。

C. EKF 和 ErKF 之间的分析差异

下面的评论试图揭示 EKF 和 ErKF 之间的一些潜在差异。
备注2.6 ErKF 仅用于估计状态向量中的误差，例如，估计 AHRS 问题的欧拉角中的误差。在这样做时，ErKF 重构了方程（22）给出的非线性域问题到公式（18）中给出的时变线性域误差估计问题，而 EKF 试图估计由公式（22）表示的系统的状态。 ErKF 公式中的系统矩阵 F(x)F(x)F(x)是实际时变动力学状态而非误差状态的函数，因此被认为是时变的，但在其他方面是线性的。 EKF 式中的系统矩阵F(x)F(x)F(x)是形成 EKF 状态向量的动力学状态的函数。因此，这个矩阵是一个非线性进行线性化后的矩阵，与 ErKF 的情况不同，它是一个本质线性的矩阵。

备注2.7 ErKF 的状态误差协方差预测遵循方程（28）。其中可以看出状态转移矩阵是由方程（18）给出的线性时变系统矩阵的离散化版本。另一方面，EKF 的状态误差协方差预测使用方程（11）的第四个方程。其中系统矩阵是关于当前状态轨迹的非线性动力学的线性化版本。因此，从最小化均方根误差的角度来看，EKF 中的状态误差协方差预测是次优的，而 ErKF 中的状态误差协方差预测是最优的。

备注2.8 ErKF 或 EKF 的状态误差协方差预测用作计算卡尔曼增益矩阵的输入，如示对于 ErKF 的方程（31）或对于 EKF 的方程（13）所示。因此，预测状态误差协方差中最优性的任何损失都会导致卡尔曼增益矩阵的计算以及基于卡尔曼增益的状态估计的更新的最优性的损失。

这段注释相当的重要

姿态航向参考系统

不重要，有空会更！

仿真结果

不重要，有空会更！

结论

当状态和测量动力学本质上是线性时，卡尔曼滤波器是最优的的最小均方估计器，前提是过程和测量噪声过程被建模为白高斯噪声。然而，当过程模型或状态传播模型是非线性时，传统上会使用一类称为扩展卡尔曼滤波器 (EKF) 的算法。 EKF 的工作原理是线性化非线性动力学，以便将卡尔曼滤波器应用于状态误差协方差预测和状态估计更新。然而，虽然 EKF 已被证明适用于多种应用，但仍受线性化误差的影响。另一种基于 KF 的方法称为误差状态 KF 或者ErKF 已经被提出和使用，特别是在机器人和飞行器姿态估计中，它以与 EKF 不同的方式使非线性动力学适应 KF。 ErKF 估计状态的误差，事实证明，在一些相当合理的假设下，误差传播动力学可以显示为线性的。因此，当使用 KF 估计误差状态而不是完整状态本身时实现了最优的更新和预测，因为（误差）过程模型是线性的。本文比较了基于 EKF 和 ErKF 滤波器的飞行器姿态估计方法，适用于三种不同的模拟机动类别。从我们的模拟中观察到，对于一组现实的过程和测量噪声协方差调定参数，ErKF 始终获得比 EKF 更好的姿态估计，并且在某些情况下明显优于 EKF。此外，据观察，为了从 EKF 中获得最佳结果，即低误差，它需要对每一类机动进行单独调整，而 ErKF 对每个机动进行相同调整时都能获得令人满意的性能。 ErKF 在关键任务期间姿态估计问题中的另一个优点是它对滤波故障更加鲁棒，即，即使滤波器失败，仍然可以通过航位推算（不补偿错误）获得姿态，而这在 EKF 中是不可能的，因为它在状态向量中包含姿态角本身。

从理论上来讲EKF和ErKF的区别是对精确问题的近似解和对近似问题的精确解的区别，根据本文的实验以及一些理论知识可以看到，后者是明显优于前者的。并且对于使用冗余参数进行旋转参数化的应用情况来说，ErKF的协方差可以给出更加具有物理意义也更加方便使用的协方差的值。