CN103312297A - 一种迭代扩展增量卡尔曼滤波方法 - Google Patents

Abstract

一种迭代扩展增量卡尔曼滤波方法，它包括以下步骤：(1)针对工程问题建立非线性离散增量系统；（2）对非线性离散增量系统进行线性化,得到线性化后的状态方程和增量量测方程；(3)对非线性离散增量系统的状态和误差方差阵进行初始化和时间更新；（4）进行量测更新时使用迭代方法；（5）得到相应的状态估计值和估计的误差方差阵。通过上述五个步骤，达到消除量测方程中未知的系统误差和建立精确的增量量测方程的目的，而且利用迭代方法优化对非线性量测方程进行线性化产生的误差，从而减少了发散现象，提高了滤波精度，增强了滤波稳定性。

Description

一种迭代扩展增量卡尔曼滤波方法

技术领域

本发明属于非线性滤波技术领域，具体涉及一种迭代扩展增量卡尔曼滤波方法。

背景技术

扩展卡尔曼滤波（EKF）目前已广泛应用于飞机和舰船的惯性导航系统、组合导航系统、卫星轨道/姿态的估计系统和探测器的导航系统等非线性动态系统中，对这类系统进行有效的状态估计。

扩展卡尔曼滤波方法的算法结构与线性卡尔曼滤波方法基本相同，都是通过对状态估计值和协方差进行时间更新和量测更新得到滤波结果。但是，扩展卡尔曼滤波方法对量测方程的精度要求较高，否则在滤波过程中会产生较大递推误差，甚至导致发散。在工程实际中，由于环境因素的影响、测量设备的不稳定性、模型和参数的选取不当等原因往往带有未知的系统误差，从而影响量测数据的精准性。但是，传统的扩展卡尔曼滤波方法对这种未知的系统误差无法进行补偿和校正，造成滤波精度低，甚至导致发散。文献“欠观测条件下的扩展增量卡尔曼滤波方法[J].航空动力学报,2012,27(4):777-781”针对非线性系统提出了一种扩展增量卡尔曼滤波方法，成功地消除了这种未知的系统误差，有效地提高了滤波精度。但是由于量测方程的非线性，使用泰勒级数展开进行线性化时不可避免地存在省略二阶以上项的误差，这些误差也会影响滤波的精度，造成滤波的不稳定性等问题。

发明内容  一种迭代扩展增量卡尔曼滤波方法

本发明的目的是提供一种迭代扩展增量卡尔曼滤波方法(Iterated EIKF).为了改进现有扩展卡尔曼滤波方法不能消除未知的系统误差，滤波精度低甚至发散的现象.该方法利用量测方程的增量形式来消除量测方程中未知的系统误差，并利用迭代方法来优化对非线性量测方程进行线性化产生的误差，从而达到减少发散现象，提高滤波精度，增强滤波稳定性的效果。

本发明一种迭代扩展增量卡尔曼滤波方法，它包括以下五个步骤：

(a)建立非线性离散增量系统

针对工程实践中所遇到非线性离散增量系统中的状态方程和量测方程，特别是具有加性噪声的系统噪声和量测噪声的非线性系统，将初始量测数据做差处理，得到相应的非线性离散增量系统，该系统为：

X_k=f_k-1(X_k-1)+W_k-1           （1）

ΔZ_k=h_k(X_k)-h_k-1(X_k-1)+V_k      （2）

式中,X_k为状态向量；f_k(·)为非线性离散函数；W_k为系统噪声向量；ΔZ_k=Z_k-Z_k-1为量测向量增量；Z_k为量测向量；h_k(·)为非线性离散函数；V_k为量测噪声向量；k是指第k步，代表t_k时刻。系统噪声向量W_k和量测噪声向量V_k的方差阵分别为Q_k和R_k，并且W_k和V_k满足

$\left.\begin{array}{c}E\left[W_k\right]=0\\ \mathrm{Cov}[W_k,W_j]=E\left[W_k{W}_j^T\right]=Q_k{\mathrm{\δ}}_{\mathrm{kj}}\\ E\left[V_k\right]=0\\ \mathrm{Cov}[V_k,V_j]=E\left[V_k{V}_j^T\right]=R_k{\mathrm{\δ}}_{\mathrm{kj}}\\ \mathrm{Cov}[W_k,V_j]=E\left[W_k{V}_j^T\right]=0\end{array}\right\}---\left(3\right)$

在工程实际中，通常量测向量Z_k存在系统误差，而量测向量增量ΔZ_k=Z_k-Z_k-1的系统误差为相对小量，可以忽略不计；

（b）对非线性离散增量系统进行线性化：首先，将式（1）中的非线性离散函数f_k-1(X_k-1)围绕估计值

展开成泰勒级数，并略去二阶以上项，得线性化之后的状态方程

X_k=Φ_k/k-1X_k-1+U_k-1+W_k-1          （4）

式中，

${\mathrm{\Φ}}_{k/k-1}=\frac{{\∂f}_{k-1}\left(X_{k-1}\right)}{{\∂X}_{k-1}}{|}_{X_{k-1}={\hat{X}}_{k-1}}---\left(5\right)$

$U_{k-1}=f_{k-1}\left({\hat{X}}_{k-1}\right)-\frac{{\∂f}_{k-1}\left(X_{k-1}\right)}{{\∂X}_{k-1}}{|}_{X_{k-1}={\hat{X}}_{k-1}}\·{\hat{X}}_{k-1}---\left(6\right)$

然后，再将式（2）中的非线性离散函数h_k(X_k)-h_k-1(X_k-1)围绕估计值

展开成泰勒级数，并略去二阶以上项，得线性化之后的增量量测方程

ΔZ_k=H_kX_k-H_k-1X_k-1+Y_k+V_k        （7）

式中，

$H_k=\frac{{\∂h}_k\left(X_k\right)}{{\∂X}_k}{|}_{X_k={\hat{X}}_{k/k-1}}---\left(8\right)$

$H_{k-1}=\frac{{\∂h}_{k-1}\left(X_{k-1}\right)}{{\∂X}_{k-1}}{|}_{X_{k-1}={\hat{X}}_{k/k-1}}---\left(9\right)$

$Y_k=h_k\left({\hat{X}}_{k/k-1}\right)-h_{k-1}\left({\hat{X}}_{k-1}\right)-(H_k\·{\hat{X}}_{k/k-1}-H_{k-1}\·{\hat{X}}_{k-1})---\left(10\right)$

通过上述过程，就得到了线性化后的状态方程和增量量测方程，式中U_k和Y_k可看作确定性输入矩阵。

(c)对由式（4）和式（7）组成的线性化之后的非线性离散增量系统的状态和误差方差阵进行初始化和时间更新：首先，对状态和误差方差阵分别赋予初值

${\hat{X}}_0=E\left[X_0\right]---\left(11\right)$

$P_0=E\left[(X_0-{\hat{X}}_0){(X_0-{\hat{X}}_0)}^T\right]---\left(12\right)$

其次，设第k-1步即t_k-1时刻的状态估计值和误差方差阵分别为在此基础之上对第k步即t_k时刻的状态向量X_k和误差方差阵P_k进行时间更新分别得到状态的一步预测

${\hat{X}}_{k/k-1}=f_{k-1}\left({\hat{X}}_{k-1}\right)---\left(13\right)$

和相应的一步预测的误差方差阵

$P_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{P}_{k-1}{\mathrm{\Φ}}_{k/k-1}^T+Q_{k-1}---\left(14\right)$

(d)使用迭代方法进行量测更新：当i=1,2,3,…时，循环进行如下步骤：

(1)计算滤波的第i步增益矩阵

：

$K_k^i=[P_{k/k-1}{\left(H_k^i\right)}^T-{\mathrm{\Φ}}_{k/k-1}{P}_{k-1}{\left(H_{k-1}^i\right)}^T]{\left({\mathrm{\Ω}}_k^i\right)}^{-1}---\left(15\right)$

(2)计算第i步量测信息残差

${\widetilde{\mathrm{\ΔZ}}}_k^i=\mathrm{\ΔZ}-[h_k\left({\hat{X}}_k^{i-1}\right)-h_{k-1}\left({\hat{X}}_{k-1}\right)]---\left(16\right)$

(3)得到第i步的状态估计

${\hat{X}}_k^i={\hat{X}}_{k/k-1}+K_k^i\{{\widetilde{\mathrm{\ΔZ}}}_k^i-H_K^i[{\hat{X}}_{k/k-1}-{\hat{X}}_k^{i-1}\left]\right\}---\left(17\right)$

式中，

$H_k^i=\frac{{\∂h}_k\left(X_k\right)}{{\∂X}_k}{|}_{X_k={\hat{X}}_k^{i-1}}---\left(18\right)$

$H_{k-1}^i=\frac{{\∂h}_{k-1}\left(X_{k-1}\right)}{{\∂X}_{k-1}}{|}_{X_{k-1}={\hat{X}}_{k-1}}---\left(19\right)$

${\mathrm{\Ω}}_h^i=H_k^i{P}_{k/k-1}{\left(H_k^i\right)}^T+R_k-H_{k-1}^i{P}_{k-1}{\mathrm{\Φ}}_{k/k-1}^T{\left(H_k^i\right)}^T-$

$H_k^i{\mathrm{\Φ}}_{k/k-1}{P}_{k-1}{\left(H_{k-1}^i\right)}^T+H_{k-1}^i{P}_{k-1}{\left(H_{k-1}^i\right)}^T$ $\left(20\right)$

在上述使用迭代方法进行量测更新过程中，当所得状态向量的估计值满足向量的2范数满足的条件（其中阈值设为ε_limit）:

${\left|\right|{\hat{X}}_k^i-{\hat{X}}_k^{i-1}\left|\right|}_2<{\mathrm{\&epsiv;}}_{\mathrm{limit}}---\left(21\right)$

时即停止计算。

(e)利用步骤（d）中的状态估计值

及相应的参数，给出估计的误差方差阵

$P_k=P_{k/k-1}-K_k^i{\mathrm{\&Omega;}}_k^i{\left(K_k^i\right)}^T---\left(22\right)$

通过上述(a)、(b)、(c)、(d)、(e)五个步骤，达到消除量测方程中未知的系统误差和建立精确的增量量测方程的目的，而且利用迭代方法优化对非线性量测方程进行线性化产生的误差，从而减少了发散现象，提高了滤波精度，增强了滤波稳定性。

其中，步骤(a)中的初始量测数据做差处理是指将当前的测量值减去上一步的测量值，也即量测向量增量是通过公式ΔZ_k=Z_k-Z_k-1得到。(a)中的k取值为k=1,2,3,…,N，其中N由滤波时间和采样周期决定。比如当滤波时间为50秒，采样周期为1赫兹时，N=50/1=50。

其中，步骤(b)中的泰勒级数在数学上是一个无穷可微的函数f(x)的幂级数展开式：

式中，n!表示n的阶乘,而f⁽ⁿ⁾(a)表示函数f(x)在点x=a处的n阶导数。实际应用中，泰勒级数需要截断，只取有限项，因此会产生相应的截断误差。

其中，步骤(d)中的向量x=(x₁,x₂,…,x_n)的2范数是x中各个元素平方之和再开根号，即

${\left|\right|x\left|\right|}_2=\sqrt{x_1^2+x_2^2\&CenterDot;\&CenterDot;\&CenterDot;+x_n^2}---\left(24\right)$

本发明的有益效果是：本发明利用量测方程的增量形式消除了量测方程中未知的系统误差，从而建立了精确的增量量测方程。在此基础之上，利用迭代方法来优化对非线性量测方程进行线性化产生的误差，从而达到减少发散现象，提高滤波精度，增强滤波稳定性的效果。本发明提出的方法计算简单，便于工程应用。经仿真验证，滤波精度提高了54%，误差均值为0.0029，误差方差为0.0012，具有良好的稳定性。

附图说明

图1是本发明的迭代扩展增量卡尔曼滤波方法的流程图；

图2是本发明的迭代扩展增量卡尔曼滤波方法中步骤三的迭代量测更新流程图；

图3是利用本发明方法与现有滤波方法的一次独立试验的状态估计结果对比图；

图4是利用本发明方法与现有滤波方法的状态误差对比图。

具体实施方式

下面结合附图和实施例对本发明作详细说明。

本发明提出的一种迭代扩展增量卡尔曼滤波方法(Iterated EIKF)，其流程图图1和迭代量测更新流程图图2所示，它包括以下五个步骤:

步骤一:针对工程实践中所遇到非线性离散增量系统中的状态方程和量测方程，特别是具有加性噪声的系统噪声和量测噪声的非线性系统，将初始量测数据做差处理，建立相应的非线性离散增量系统，该系统为：

X_k=f_k-1(X_k-1)+W_k-1             （1）

ΔZ_k=h_k(X_k)-h_k-1(X_k-1)+V_k         （2）

式中,X_k为状态向量；f_k(·)为非线性离散函数；W_k为系统噪声向量；ΔZ_k=Z_k-Z_k-1为量测向量增量；Z_k为量测向量；h_k(·)为非线性离散函数；V_k为量测噪声向量；k是指第k步，代表t_k时刻。系统噪声向量W_k和量测噪声向量V_k的方差阵分别为Q_k和R_k，并且W_k和V_k满足

$\left.\begin{array}{c}E\left[W_k\right]=0\\ \mathrm{Cov}[W_k,W_j]=E\left[W_k{W}_j^T\right]=Q_k{\mathrm{\&delta;}}_{\mathrm{kj}}\\ E\left[V_k\right]=0\\ \mathrm{Cov}[V_k,V_j]=E\left[V_k{V}_j^T\right]=R_k{\mathrm{\&delta;}}_{\mathrm{kj}}\\ \mathrm{Cov}[W_k,V_j]=E\left[W_k{V}_j^T\right]=0\end{array}\right\}---\left(3\right)$

步骤二:此步骤是在步骤一的基础之上对非线性离散增量系统进行线性化。首先，将式（1）中的非线性离散函数f_k-1(X_k-1)围绕估计值

展开成泰勒级数，仅保留一阶项，将二阶及以上项略去，便得线性化之后的状态方程

X_k=Φ_k/k-1X_k-1+U_k-1+W_k-1            （4）

式中，

${\mathrm{\&Phi;}}_{k/k-1}=\frac{{\&PartialD;f}_{k-1}\left(X_{k-1}\right)}{{\&PartialD;X}_{k-1}}{|}_{X_{k-1}={\hat{X}}_{k-1}}---\left(5\right)$

$U_{k-1}=f_{k-1}\left({\hat{X}}_{k-1}\right)-\frac{{\&PartialD;f}_{k-1}\left(X_{k-1}\right)}{{\&PartialD;X}_{k-1}}{|}_{X_{k-1}={\hat{X}}_{k-1}}\&CenterDot;{\hat{X}}_{k-1}---\left(6\right)$

然后，再将式（2）中的非线性离散函数h_k(X_k)-h_k-1(X_k-1)围绕估计值展开成泰勒级数，仅保留一阶项，将二阶及以上项略去，便得线性化之后的增量量测方程

ΔZ_k=H_kX_k-H_k-1X_k-1+Y_k+V_k        （7）

式中，

$H_k=\frac{{\&PartialD;h}_k\left(X_k\right)}{{\&PartialD;X}_k}{|}_{X_k={\hat{X}}_{k/k-1}}---\left(8\right)$

$H_{k-1}=\frac{{\&PartialD;h}_{k-1}\left(X_{k-1}\right)}{{\&PartialD;X}_{k-1}}{|}_{X_{k-1}={\hat{X}}_{k/k-1}}---\left(9\right)$

$Y_k=h_k\left({\hat{X}}_{k/k-1}\right)-h_{k-1}\left({\hat{X}}_{k-1}\right)-(H_k\&CenterDot;{\hat{X}}_{k/k-1}-H_{k-1}\&CenterDot;{\hat{X}}_{k-1})---\left(10\right)$

通过上述过程，就得到了线性化之后的状态方程和增量量测方程，式中U_k和Y_k可看作确定性输入矩阵。

其中，利用泰勒级数对非线性离散函数进行泰勒级数展开时，只取前两项。比如，对一个无穷可微的函数f(x)进行泰勒级数展开，只取f(x)=f(a)+f′(a)(x-a)。

以上两个步骤为前期准备过程，下面三个步骤是迭代扩展增量卡尔曼滤波方法的滤波过程。

步骤三:此步骤中主要对由式（4）和式（7）组成的线性化之后的非线性离散增量系统的状态和误差方差阵进行初始化和时间更新：首先，对状态和误差方差阵分别赋予初值

${\hat{X}}_0=E\left[X_0\right]---\left(11\right)$

$P_0=E\left[(X_0-{\hat{X}}_0){(X_0-{\hat{X}}_0)}^T\right]---\left(12\right)$

其次，设第k-1步即t_k-1时刻的状态估计值和误差方差阵分别为在此基础之上对第k步即t_k时刻的状态向量X_k和误差方差阵P_k进行时间更新分别得到状态的一步预测

${\hat{X}}_{k/k-1}=f_{k-1}\left({\hat{X}}_{k-1}\right)---\left(13\right)$

和相应的一步预测的误差方差阵

$P_{k/k-1}={\mathrm{\&Phi;}}_{k/k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k/k-1}^T+Q_{k-1}---\left(14\right)$

其中，k取值为k=1,2,3,…,N，其中N由滤波时间和采样周期决定。比如当滤波时间为50秒，采样周期为1秒时，N=50/1=50。

步骤四:此步骤中主要是使用迭代方法进行量测更新，增强滤波的稳定性。当i=1,2,3,…时，循环进行如下步骤(参见迭代量测更新流程图图2)：

(1)计算滤波的第i步增益矩阵

$K_k^i=[P_{k/k-1}{\left(H_k^i\right)}^T-{\mathrm{\&Phi;}}_{k/k-1}{P}_{k-1}{\left(H_{k-1}^i\right)}^T]{\left({\mathrm{\&Omega;}}_k^i\right)}^{-1}---\left(15\right)$

(2)计算第i步量测信息残差

${\widetilde{\mathrm{\&Delta;Z}}}_k^i={\mathrm{\&Delta;Z}}_k-[h_k\left({\hat{X}}_k^{i-1}\right)-h_{k-1}\left({\hat{X}}_{k-1}\right)---\left(16\right)$

(3)得到第i步的状态估计

${\hat{X}}_k^i={\hat{X}}_{k/k-1}+K_k^i\{{\widetilde{\mathrm{\&Delta;Z}}}_k^i-H_K^i[{\hat{X}}_{k/k-1}-{\hat{X}}_k^{i-1}\left]\right\}---\left(17\right)$

式中，

$H_k^i=\frac{{\&PartialD;h}_k\left(X_k\right)}{{\&PartialD;X}_k}{|}_{X_k={\hat{X}}_k^{i-1}}---\left(18\right)$

$H_{k-1}^i=\frac{{\&PartialD;h}_{k-1}\left(X_{k-1}\right)}{{\&PartialD;X}_{k-1}}{|}_{X_{k-1}={\hat{X}}_{k-1}}---\left(19\right)$

${\mathrm{\&Omega;}}_h^i=H_k^i{P}_{k/k-1}{\left(H_k^i\right)}^T+R_k-H_{k-1}^i{P}_{k-1}{\mathrm{\&Phi;}}_{k/k-1}^T{\left(H_k^i\right)}^T-$

$H_k^i{\mathrm{\&Phi;}}_{k/k-1}{P}_{k-1}{\left(H_{k-1}^i\right)}^T+H_{k-1}^i{P}_{k-1}{\left(H_{k-1}^i\right)}^T$ $\left(20\right)$

在上述使用迭代方法进行量测更新过程中，当所得状态向量的估计值

满足向量的2范数满足的条件（其中阈值设为ε_limit）:

${\left|\right|{\hat{X}}_k^i-{\hat{X}}_k^{i-1}\left|\right|}_2<{\mathrm{\&epsiv;}}_{\mathrm{limit}}---\left(21\right)$

时即停止计算。

其中，阈值ε_limit一般取一个较小的正数，它决定了对滤波的精度要求。比如取ε_limit=0.1或者ε_limit=0.001。

其中，向量x=(x₁,x₂,…,x_n)的2范数是x中各个元素平方之和再开根号，通过公式 ${\left|\right|x\left|\right|}_2=\sqrt{x_1^2+x_2^2+\&CenterDot;\&CenterDot;\&CenterDot;+x_n^2}$ 进行计算求得。

此上述迭代过程中，只取i=1时，即为扩展增量卡尔曼滤波方法（EKF）。

步骤五:利用步骤四中的状态估计值

及相应的参数，给出估计的误差方差阵

$P_k=P_{k/k-1}-K_k^i{\mathrm{\&Omega;}}_k^i{\left(K_k^i\right)}^T---\left(22\right)$

按照步骤三、步骤四和步骤五中的公式（13）至（22）进行循环计算，即为全部的迭代扩展增量卡尔曼滤波过程。

针对本发明的方法，进行仿真验证，考虑下面的一维非线性离散系统：

x_k=0.9x_k-1+w_k-1            （23）

z_k=x_k+0.001ln(x_k+1)+a+v_k       （24）

式中，w_k和v_k是独立的高斯白噪声序列，且系统噪声w_k的均值q=0和方差Q=0.1，量测噪声v_k的均值r=0和方差R=1。状态初始值为x₀=10，误差阵初值维P₀=0.1，量测系统误差a=3为未知量。采样周期为1s。

由于状态方程为线性方程，故只需要线性化增量量测方程，可得线性增量量测方程式（7），其中H_k和H_k-1分别为

$H_k=1+\frac{0.001}{{\hat{X}}_{k/k-1}+1}---\left(25\right)$

$H_{k-1}=1+\frac{0.001}{{\hat{X}}_{k-1}+1}---\left(26\right)$

从图3中可以看到，迭代扩展增量卡尔曼滤波得到的状态估计比扩展增量卡尔曼滤波的精度高，能够消除系统误差对滤波的影响，提高滤波的稳定性。图4给出两种滤波结果与真值的误差比较，滤波误差均值分别为0.0063和0.0029，误差方差分别为0.0106和0.0012，可以进一步看到，迭代扩展增量卡尔曼滤波的误差比扩展增量卡尔曼滤波的误差要小得多，能很好跟踪状态x_k的变化。

Claims (4)

1.一种迭代扩展增量卡尔曼滤波方法，其特征在于：它包括以下步骤： 

步骤一：建立非线性离散增量系统 

针对工程实践中所遇到非线性离散增量系统中的状态方程和量测方程，特别是具有加性噪声的系统噪声和量测噪声的非线性系统，将初始量测数据做差处理，得到相应的非线性离散增量系统，该系统为： 

X_k=f_k-1(X_k-1)+W_k-1         （1） 

ΔZ_k=h_k(X_k)-h_k-1(X_k-1)+V_k        （2） 

式中,X_k为状态向量；f_k(·)为非线性离散函数；W_k为系统噪声向量；ΔZ_k=Z_k-Z_k-1为量测向量增量；Z_k为量测向量；h_k(·)为非线性离散函数；V_k为量测噪声向量；k是指第k步，代表t_k时刻；系统噪声向量W_k和量测噪声向量V_k的方差阵分别为Q_k和R_k，并且W_k和V_k满足 

在工程实际中，通常量测向量Z_k存在系统误差，而量测向量增量ΔZ_k=Z_k-Z_k-1的系统误差为相对小量，忽略不计； 

步骤二：对非线性离散增量系统进行线性化：首先，将式（1）中的非线性离散函数f_k-1(X_k-1)围绕估计值

展开成泰勒级数，并略去二阶以上项，得线性化之后的状态方程 

X_k=Φ_k/k-1X_k-1+U_k-1+W_k-1        （4） 

式中， 

然后，再将式（2）中的非线性离散函数h_k(X_k)-h_k-1(X_k-1)围绕估计值

展开成泰勒级数，并略去二阶以上项，得线性化之后的增量量测方程： 

ΔZ_k=H_kX_k-H_k-1X_k-1+Y_k+V_k        （7） 

式中， 

通过上述过程，就得到了线性化后的状态方程和增量量测方程，式中U_k和Y_k看作确定性输入矩阵； 

步骤三：对由式（4）和式（7）组成的线性化之后的非线性离散增量系统的状态和误差方差阵进行初始化和时间更新：首先，对状态和误差方差阵分别赋予初值 

其次，设第k-1步即t_k-1时刻的状态估计值和误差方差阵分别为

和P_k-1，在此基础之上对第k步即t_k时刻的状态向量X_k和误差方差阵P_k进行时间更新分别得到状态的一步预测 

和相应的一步预测的误差方差阵 

步骤四：使用迭代方法进行量测更新：当i=1,2,3,…时，循环进行如下步骤： 

(1)计算滤波的第i步增益矩阵

(2)计算第i步量测信息残差

(3)得到第i步的状态估计

式中， 

在上述使用迭代方法进行量测更新过程中，当所得状态向量的估计值

满足向量的2范数满足的条件,其中阈值设为ε_limit: 

时即停止计算； 

步骤五： 利用步骤四中的状态估计值

及相应的参数，给出估计的误差方差阵 

通过上述步骤一、二、三、四、五，达到消除量测方程中未知的系统误差和建立精确的增量量测方程的目的，而且利用迭代方法优化对非线性量测方程进行线性化产生的误差，从而减少了发散现象，提高了滤波精度，增强了滤波稳定性。 

2.根据权利要求1所述的一种迭代扩展增量卡尔曼滤波方法，其特征在于：在步骤一中的初始量测数据做差处理是指将当前的测量值减去上一步的测量值，也即量测向量增量是通过公式ΔZ_k=Z_k-Z_k-1得到；步骤一中的k取值为k=1,2,3,…,N，其中N由滤波时间和采样周期决定；比如当滤波时间为50秒，采样周期为1赫兹时，N=50/1=50。 

3.根据权利要求1所述的一种迭代扩展增量卡尔曼滤波方法，其特征在于：在步骤二中的泰勒级数在数学上是一个无穷可微的函数f(x)的幂级数展开式： 

式中，n!表示n的阶乘,而f⁽ⁿ⁾(a)表示函数f(x)在点x=a处的n阶导数；实际应用中，泰勒级数需要截断，只取有限项，因此会产生相应的截断误差。 

4.根据权利要求1所述的一种迭代扩展增量卡尔曼滤波方法，其特征在于：在步骤四中的向量x=(x₁,x₂,…,x_n)的2范数是x中各个元素平方之和再开根号，即 

。

Images