一种基于状态扩维的隐函数量测模型滤波方法

摘要

本发明涉及一种基于状态扩维的隐函数量测模型滤波方法，IAUKF。在此方法中，量测量被扩展到状态量中，同时零向量被视为等效的量测量来进行滤波更新。IAUKF相较于IAEKF和IEKF，获得更好的估计性能。特别是当量测噪声增加时，相较于隐式UKF，性能可以得到很大的改善。

说明书

技术领域

本发明属于航天器自主导航领域，涉及一种状态扩维的隐函数量测模型滤波方法。

背景技术

R.E.Kalman在1960年提出了一种线性最优递推的滤波方法，即卡尔曼滤波(Kalman Filter,KF)。最初的KF仅适用于线性系统，随着人们对非线性系统滤波需求的扩大，扩展卡尔曼滤波(Extended Kalman Filter,EKF)、Unscented卡尔曼滤波(UnscentedKalman Filter,UKF)、粒子滤波(Particle Filter,PF)等滤波方法被逐步提出并取得不断的发展。经典的卡尔曼滤波算法中量测模型都具有显式表达式，然而在许多实际问题中，状态量和量测量的约束往往是隐式的，不易或不能获得显式的量测模型，这类问题就是隐式量测模型滤波问题。

国内外关于含隐式量测模型的状态估计问题的解决方法主要有两类方法。第一类是由Soatto等提出的通过隐式扩展卡尔曼滤波器(Implicit ExtendedKalman Filter,IEKF)，通过将隐式量测方程在参考点处线性化并取二阶形式，结合传统的带有显式量测方程的EKF算法，得到了IEKF。第二类是由Steffen在分析IEKF方法的基础上，提出的一种含有迭代式量测更新的隐式量测模型的滤波方法，即Iterative IEKF。上述两类方法均建立在EKF基础上，在应用时都需要计算Jacobian矩阵，线性化误差会降低滤波算法的精度，可能引起滤波结果的发散，而且Jacobian矩阵的计算通常比较复杂。

发明内容

本发明要解决的技术问题是：克服现有技术的不足，提供一种基于状态扩维的隐函数量测模型滤波方法，相较于IAEKF和IEKF，获得更好的估计性能。特别是当量测噪声增加时，相较于隐式UKF，估计性能可以得到很大的改善。

本发明提出一种基于状态扩维的隐函数量测模型滤波方法，IAUKF。在此方法中，量测量被扩展到状态量中，同时零向量被视为等效的量测量来进行滤波更新，具体包括以下步骤：

第一步，首先，将k时刻的真实量测量扩展到状态量X_k中，构造扩展后的状态量和建立扩维状态量满足的系统模型；

第二步，初始化，根据系统模型，求解初始时刻的状态量，将初始时刻的状态量及其协方差阵代入第一步中构造好的扩展后的状态量和中，分别记作和

第三步，时间更新，根据第二步中求得的扩展后的初始时刻的状态量和协方差阵，计算预测的状态及其误差协方差阵其余的时刻，在计算预测的状态及其误差协方差阵时，则要根据第四步量测更新步骤中求得的状态量以及相应的误差协方差阵来求得；

第四步，量测更新，根据时间更新求解得到的状态预测值，求解更新后的状态估计以及相应的误差协方差阵然后回到第三步，实现循环求解，直到滤波结束。

所述第一步构造扩展后的状态量，建立扩维状态量满足的系统模型如下：

其中，X_k和Z_k分别为k时刻的状态量和量测量，表示扩展后的状态量，表示由实际量测量Z_k和量测噪声v_k混合得到的真实的量测量；

式中，F^a(·)和分别表示扩展状态量的状态模型及其状态误差，h(·)是非线性显式函数。

所述第二步中，扩维后的状态量及其协方差阵初始化为：

其中，E[x]表示x的期望值，Z₁和N₁分别表示初始时刻的量测量及其协方差阵，n和m分别代表状态向量和量测向量的维数。

所述第三步，时间更新如下：

式中，是满足均值为协方差为的点，w_i是第i个Sigma点的权值，为的协方差阵，其中，

式中，n_a表示扩展后的状态量的维数，τ是标度参数，表示矩阵平方根的第i维列向量。

所述第四步，量测更新如下：

其中，为滤波增益矩阵，Y_kk-1为量测的预测值，分别为相应的误差协方差阵。

本发明与现有技术相比的优点在于：

(1)本发明把状态量和真实的量测量扩展成一个新的状态量，同时将零向量视为等效的量测量来进行滤波更新，相较于IAEKF和IEKF，获得更好的估计性能，使得估计精度得到提高；特别是当量测噪声增加时，相较于隐式UKF，性能可以得到很大的改善。

(2)本发省略了现有方法中求解雅克比矩阵的过程，减少了计算量。

附图说明

图1为本发明中基于状态扩维的隐函数量测模型滤波方法的流程图；

具体实施方式

图1给出了基于状态扩维的隐函数量测模型滤波方法的流程图。下面详细说明本发明的具体实施过程：

常见的非线性系统均含有显式的量测模型，此类系统可描述为：

式中，状态方程f(·)和量测方程h(·)均是非线性显式函数。X_k和w_k分别表示k时刻的状态向量及其噪声，X_k+1表示k+1时刻的状态向量。Z_k和v_k分别表示k时刻的量测向量及其噪声。实际应用中，可以认为状态向量和量测向量均受到零均值、不相关的高斯白噪声影响，即状态量噪声和量测量噪声分别服从：

其中，Q_k和N_k分别表示状态噪声和量测噪声所对应的协方差阵，其具体值由工程经验或系统参数确定。

然而在许多实际问题中，状态量和实际的量测量是被约束于一个隐函数形式的量测模型中，不易或者无法获得显式量测方程，这类问题就是隐式量测模型的滤波问题。此类隐式量测模型可描述为如下的系统：

其中，状态量X_k和量测量Z_k构成隐函数h(·)＝0约束。

首先，为了处理含有隐式量测模型的非线性系统问题，可以把状态量和真实的量测量扩展成一个新的状态量：

式中，上标a标记状态扩展，表示由实际量测量Z_k和量测噪声v_k混合得到的真实的量测量，表示扩展后的状态量。

第二，注意系统(3)中的h(X_k,Z_k+v_k)等于m维的零向量，因此可以把零向量看作是等效的量测量Y_k，即：

由于扩展后的状态量是由之前的状态量X_k和真实量测量构成的，因此量测模型(5)可以改写为：

因此，建立扩维状态量满足的系统模型为：

式中，F^a(·)和分别表示扩展状态量的状态模型及其状态误差，F^a(·)和的计算公式如下：

其中，的协方差阵定义为

IAUKF方法的具体实现步骤如下：

1.初始化

初始状态估计及相应的误差协方差阵和P₀分别设定为：

式中，E[x]表示x的期望值，状态误差协方差阵Q_k和量测误差协方差阵N_k分别选择初始时刻相应的值。

初始的扩展状态量应该根据式(4)构造，然而在实际应用时，真实量测量无法获得，本发明用实际的量测量Z₁代替真实量测量此时引入了噪声协方差阵为N₁的量测噪声，因此，扩维后的状态量及其协方差阵可以分别构造为：

式中，Z₁和N₁分别表示初始时刻的量测量及其协方差阵，n和m分别代表状态向量和量测向量的维数。

2.时间更新

在k时刻，首先需要对上一时刻获得的扩展状态量估计值根据式(4)进行修正。然而，在实际应用时，真实量测量无法获得，我们用实际的量测量Z_k代替真实量测量此时，引入了量测噪声v_k及其噪声协方差阵N_k。因此，扩维状态量及其协方差阵可以按下式进行修正：

式中，和P_k-1分别表示包含于和中的未扩展的状态量估计及其误差协方差阵。

同样地，IAUKF方法基于UT变换进行概率推演。满足均值为协方差为的2n_a+1个Sigma点等效于的分布，这些Sigma点通过系统模型(7)传播可以得到对应传播后的Sigma点，这些传播后的Sigma点可用于计算预测的状态及其误差协方差阵这组特定的Sigma按照下式确定：

式中，n_a表示扩展后的状态量的维数，数值上等于n+m，τ是标度参数，表示矩阵平方根的第i维列向量，w_i是第i个Sigma点的权值。

Sigma点按状态模型传递为：

则状态预测值及其误差协方差阵可按下式计算：

3.量测更新

按照式(6)，估计量测量的Sigma点可以按照下式计算得到：

由于根据式(16)得到的Sigma点的预测值并不是k时刻的真实状态，含有误差。因此，式(18)也不等于其真实值0，这提供了关于预测状态的误差信息，可用于状态修正。

于是，量测的预测值可计算为：

相应的误差协方差阵可由下式获得：

然后，滤波增益矩阵更新后的状态估计以及相应的误差协方差阵可分别根据UKF方法计算获得：

表1，表2为IAUKF，IEKF,IAEKF,和隐式UKF的导航结果比较。

表1四种滤波方法的导航结果

表2不同量测噪声下四种滤波方法的导航结果

表1所比较的是当量测噪声比较小，为1″时，四种滤波方法的滤波结果；表2所比较的是，不同量测噪声下，四种滤波方法的滤波结果。可以看出：量测噪声较小时，IAUKF相较于IAEKF和IEKF，位置的估计精度得到很大提高；当量测噪声逐渐增加时，IAUKF相较于隐式UKF，估计性能可以得到很大的改善。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。