CN103217175A - 一种自适应容积卡尔曼滤波方法 - Google Patents

Abstract

本发明涉及的是一种自适应容积卡尔曼滤波方法，特别是涉及一种带渐消记忆时变噪声统计估值器的自适应容积卡尔曼滤波方法。本发明包括下列步骤：（1）设定初始参数；（2）时间更新；（3）量测更新；（4）构造渐消记忆时变噪声统计估值器；（5）实时估计和修正噪声。相比于标准容积卡尔曼滤波方法，该方法不要求精确已知噪声的先验统计特性，具有应对噪声变化的自适应能力，且噪声统计估值器递推公式简单，更容易实现，且对噪声统计的估计是无偏的。

Description

一种自适应容积卡尔曼滤波方法

技术领域

本发明涉及的是一种自适应容积卡尔曼滤波方法，特别是涉及一种带渐消记忆时变噪声统计估值器的自适应容积卡尔曼滤波方法。

背景技术

加拿大学者Arasaratnam在文献《Cubature Kalman Filters》（IEEE Transactions onAutomatic Control,2009,54(6):1254-1269）中提出了一种新型非线性滤波方法：容积卡尔曼滤波器（Cubature Kalman Filter，即CKF）。容积卡尔曼滤波是根据贝叶斯理论以及容积规则经过严格的数学推导得出的滤波算法，在理论上有保证，其根据容积准则，通过一组具有相同权重的点经过非线性***方程转换后产生新的点来给出下一时刻***状态的预测，避免了对非线性模型的线性化处理，其精度达三阶。由于容积卡尔曼滤波估计精度高，不容易发散且计算量小的优点，容积卡尔曼滤波一提出就迅速被各领域的学者所接纳，将其用于许多估计问题中。

文献《Cubature Kalman Filtering for Continuous-Discrete Systems:Theory and Simulations》（IEEE Transactions on Signal Processing.2010,58(10):4977-4993P）将容积卡尔曼滤波用于处理空中交通管制情况下需跟踪机动情形目标飞行器的轨迹问题，该情况下的状态模型同时具有连续与离散性质，仿真结果显示，相对于无迹卡尔曼滤波，容积卡尔曼滤波能够容许目标飞行器更大的机动角速度以及量测噪声更长的采样时间。

文献《Cubature Kalman Filter based Localization and Mapping》（The18^th IFAC WorldCongress.2011,2121-2125P）将容积卡尔曼滤波用于同步定位与地图构建算法中，与基于无迹卡尔曼滤波的同步定位与地图构建算法相比，基于容积卡尔曼滤波的同步定位与地图构建算法对目标位置的最大估计误差降低了近40%。

文献《A CKF Based Spatial Alignment of Radar and Infrared Sensors》（IEEE10^thInternational Conference on Signal Processing.2010,2386-2390P）指出在雷达与红外传感器间的偏差估计算法中，对俯仰角的估计，容积卡尔曼滤波比无迹卡尔曼滤波具有更小的误差及更快的收敛性。

然而，标准容积卡尔曼滤波作为高斯滤波器，同扩展卡尔曼滤波和无迹卡尔曼滤波一样，需要预知***的数学模型和噪声统计的先验知识。而在实际应用中，噪声的先验统计是未知或不准确的，即使已知噪声的先验统计特性，由于内、外部不确定因素的影响，使得噪声统计特性极易发生改变，体现出较强的时变特性。标准容积卡尔曼滤波不具有应对噪声统计变化的自适应能力，其在噪声统计未知时变情况下，可能导致较大的估计误差，甚至导致滤波器发散。因此，需要引入自适应估计方法对容积卡尔曼滤波进行改进使其不依赖于先验噪声统计。

发明内容

本发明的目的在于提供一种不要求精确已知噪声的先验统计特性，具有应对噪声变化的自适应能力，且噪声统计估值器递推简单，容易实现的带渐消记忆时变噪声统计估值器的自适应容积卡尔曼滤波方法。

本发明的目的是这样实现的：

本发明包括下列步骤：

（1）设定初始参数：

设定初始时刻***状态值x₀，初始时刻状态协方差P₀，初始时刻***噪声均值q的渐消记忆时变噪声统计估值器

初始时刻***噪声协方差Q的渐消记忆时变噪声统计估值器初始时刻***噪声均值r的渐消记忆时变噪声统计估值器初始时刻***噪声协方差R的渐消记忆时变噪声统计估值器

遗忘因子b；

（2）时间更新：

进行自适应容积卡尔曼滤波基于标准容积卡尔曼滤波的时间更新：

$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T,$

$X_{i,k-1|k-1}=S_{k-1|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1|k-1},$

$x_{i.k-1|k-1}^{*}=f\left(X_{i,k-1|k-1}\right),$

在k时刻的状态预测值

中融入***噪声均值q的渐消记忆时变噪声统计估值器

${\hat{x}}_{k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}f\left(X_{i,k-1|k-1}\right)+{\hat{q}}_{k-1},$

在k时刻的误差协方差预测值P_k|k-1中融入***噪声协方差差Q的渐消记忆时变噪声统计估值器

$P_{k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{X}_{i,k|k-1}^{*}{X}_{i,k|k-1}^{*T}-{\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}^T+{\hat{Q}}_{k-1},$

其中，P_k-1|k-1为k-1时刻的误差协方差；S_k-1|k-1为P_k-1|k-1乔里斯基因子；X_i,k-1|k-1为容积点集；m=2n，n为状态向量的维数；

记n维单位列向量e=[1,0,…,0]^T，使用符号[1]表示对e的元素进行全排列和改变元素符号产生的点集，称为完整全对称点集，[1]_i表示点集[1]中的第i个点；

为k-1时刻的状态估计值；

为通过状态方程传播的容积点集；

为非线性状态方程；

为k时刻的状态预测值；P_k|k-1为k时刻的状态误差协方差预测值；

为***噪声均值q的渐消记忆时变噪声统计估值器；

为***噪声协方差Q的渐消记忆时变噪声统计估值器；

（3）量测更新：

进行自适应容积卡尔曼滤波基于标准容积卡尔曼滤波的量测更新：

$P_{k|k-1}=S_{k|k-1}{S}_{k|k-1}^T,$

$X_{i,k|k-1}=S_{k|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k|k-1},$

$z_{i,k|k-1}=h\left(X_{i,k|k-1}\right),$

在k时刻的量测预测值

中融入量测噪声均值r的渐消记忆时变噪声统计估值器

${\hat{z}}_{k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}h\left(X_{i,k|k-1}\right)+{\hat{r}}_k,$

在自相关协方差矩阵P_zz,k|k-1中融入量测噪声协方差R的渐消记忆时变噪声统计估值器

$P_{\mathrm{zz},k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{z}_{i,k|k-1}{z}_{i,k|k-1}^T-{\hat{z}}_{k|k-1}{z}_{k|k-1}^T+{\hat{R}}_k,$

结合标准容积卡尔曼滤波中互相关协方差矩阵P_xz,k|k-1，卡尔曼增益K_k，k时刻的状态估计值

k时刻的状态误差协方差估计值P_k|k的完成量测更新：

$P_{\mathrm{xz},k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{X}_{i,k|k-1}{z}_{i,k|k-1}^T-{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T,$

$K_k=P_{\mathrm{xz},k|k-1}{P}_{\mathrm{zz},k|k-1}^{-1},$

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1}),$

$P_{k|k}=P_{k|k-1}-K_k{P}_{\mathrm{zz},k|k-1}{K}_k^T,$

其中，S_k|k-1为P_k|k-1乔里斯基因子；X_i,k|k-1为容积点集；z_i,k|k-1为通过量测方程传播的容积点集；

为非线性量测函数；

为k时刻的量测预测值；为量测噪声均值r的渐消记忆时变噪声统计估值器；P_zz,k|k-1为自相关协方差矩阵；

为量测噪声协方差R的渐消记忆时变噪声统计估值器；P_xz,k|k-1为互相关协方差矩阵；K_k为卡尔曼增益；

为k时刻的状态估计值；z_k为k时刻的量测值；P_k|k为k时刻的状态误差协方差估计值；

（4）构造渐消记忆时变噪声统计估值器：

构造次优MAP常值噪声统计估值器；

分析次优MAP常值噪声统计估值器的无偏性，构造次优无偏MAP常值噪声统计估值器；

在次优无偏MAP常值噪声统计估值器的基础上，借助渐消记忆指数加权方法，构造的渐消记忆时变噪声统计估值器

和

${\hat{q}}_k=(1-d_k){\hat{q}}_{k-1}+d_k[{\hat{x}}_{k|k}-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}f\left(X_{i,k-1|k-1}\right)],$

${\hat{Q}}_k=(1-d_k){\hat{Q}}_{k-1}+d_k[K_k{\mathrm{\γ}}_k{\mathrm{\γ}}_k^T{K}_k^T+P_{k|k}-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{X}_{i,k|k-1}{z}_{i,k|k-1}^T+{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T],$

${\hat{r}}_k=(1-d_k){\hat{r}}_{k-1}+d_k[z_k-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}h\left(X_{i,k|k-1}\right)],$

${\hat{R}}_k=(1-d_k){\hat{R}}_{k-1}+d_k[{\mathrm{\γ}}_k{\mathrm{\γ}}_k^T-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{z}_{i,k|k-1}{z}_{i,k|k-1}^T+{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T],$

其中，d_k=(1-b)/(1-b^k)，b为遗忘因子，且满足0<b<1；γ_k为k时刻的残差；

（5）实时估计和修正噪声：

利用步骤（4）中所描述的渐消记忆时变噪声统计估值器

和

对***噪声Q和量测噪声R进行实时估计和修正。

本发明的有益效果在于：

本发明的主要贡献在于针对容积卡尔曼滤波在噪声统计未知时变情况下易出现滤波精度下降甚至发散的问题，提出带渐消记忆时变噪声统计估值器的自适应容积卡尔曼滤波方法，该方法在进行滤波计算的同时，采用渐消记忆时变噪声统计估值器对未知时变的噪声统计特性进行实时估计和修正，相比于标准容积卡尔曼滤波方法，该方法不要求精确已知噪声的先验统计特性，具有应对噪声变化的自适应能力，且噪声统计估值器递推公式简单，容易实现，且对噪声统计的估计是无偏的。

附图说明

图1为算法设计框图；

图2为全局坐标系与无人水下航行器船体坐标系；

图3为***噪声定常情况下估计轨迹对比曲线；

图4为***噪声定常情况下东向误差曲线；

图5为***噪声定常情况下北向误差曲线；

图6为***噪声时变情况下估计轨迹对比曲线；

图7为***噪声时变情况下东向误差曲线；

图8为***噪声时变情况下北向误差曲线；

图9为无人水下航行器艏向的变化曲线。

具体实施方式

下面给出本发明的优选实施方式，并结合附图和无人水下航行器（Underwater UnmannedVehicle，即UUV）海试例证加以说明。

如附图1所示，本发明的是通过如下步骤实现的：

考虑如下离散时间非线性动态***：

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+w_{k-1}\\ z_k=h\left(x_k\right)+v_k\end{array}\right.---\left(1\right)$

式中，x_k和z_k分别为***的n维状态向量和m维量测向量；函数f(·)和h(·)分别为非线性***状态方程和量测方程；w_k和v_k分别为n维***噪声和m维量测噪声。

假设1.w_k和v_k为互不相关的高斯白噪声，其具有如下常值统计特性：

$\left\{\begin{array}{cc}E\left(w_k\right)=q,& \mathrm{cov}(w_k,w_j^T)=Q{\mathrm{\&delta;}}_{\mathrm{kj}}\\ E\left(v_k\right)=r,& \mathrm{cov}(v_k,v_j^T)=R{\mathrm{\&delta;}}_{\mathrm{kj}}\\ \mathrm{cov}(w_k,v_j^T)=0& \end{array}\right.---\left(2\right)$

其中，δ_kj为kronecker-δ函数，Q是非负定对称阵，R是正定对称阵。

假设2.初始状态x₀与所有噪声互不相关，且服从高斯正态分布，其先验均值和协方差矩阵为：

$\left\{\begin{array}{c}{\hat{x}}_0=E\left(x_0\right)\\ P_0=\mathrm{cov}(x_0,x_0^T)=E(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\end{array}\right.---\left(3\right)$

假设3.w_k和v_k是互不相关的高斯白噪声，且具有如下时变统计特性：

$\left\{\begin{array}{cc}E\left(w_k\right)=q_k,& \mathrm{cov}(w_k,w_j^T)=Q_k{\mathrm{\&delta;}}_{\mathrm{kj}}\\ E\left(v_k\right)=r_k,& \mathrm{cov}(v_k,v_j^T)=R_k{\mathrm{\&delta;}}_{\mathrm{kj}}\\ \mathrm{cov}(w_k,v_j^T)=0& \end{array}\right.---\left(4\right)$

其中，Q_k是非负定对称阵，R_k是正定对称阵。

容积卡尔曼滤波在滤波过程中，假定***噪声和量测噪声的均值为零。若***噪声和量测噪声的均值不为零，令μ_k=w_k-q，η_k=v_k-r，代入非线性***(1)中，可得：

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+q+{\mathrm{\&mu;}}_{k-1}\\ z_k=h\left(x_k\right)+r+{\mathrm{\&eta;}}_k\end{array}\right.---\left(5\right)$

其中，μ_k和η_k都是均值为0、方差分别为Q和R的高斯白噪声。

基于非线性***(5)的自适应容积卡尔曼滤波具体流程如下：

1设定初始参数

设定初始时刻k=0状态值x₀，初始时刻状态协方差P₀，初始时刻***噪声均值q的渐消记忆时变噪声统计估值器

初始时刻***噪声协方差Q的渐消记忆时变噪声统计估值器初始时刻***噪声均值r的渐消记忆时变噪声统计估值器

初始时刻***噪声协方差R的渐消记忆时变噪声统计估值器

遗忘因子b。

2时间更新

假设k-1时刻后验概率密度函数

已知，通过乔里斯基分解误差协方差P_k-1|k-1：

$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T---\left(6\right)$

计算容积点（i=1,2,…,m；m=2n）：

$X_{i,k-1|k-1}=S_{k-1|k-1}{\mathrm{\&xi;}}_i+{\hat{x}}_{k-1|k-1}---\left(7\right)$

通过状态方程传播容积点：

$X_{i,k-1|k-1}^{*}=f\left(X_{i,k-1|k-1}\right)---\left(8\right)$

估计k时刻的状态预测值：

${\hat{x}}_{k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}f\left(X_{i,k-1|k-1}\right)+{\hat{q}}_{k-1}---\left(9\right)$

估计k时刻的状态误差协方差预测值：

$P_{k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{X}_{i,k|k-1}^{*}{X}_{i,k|k-1}^{*T}-{\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}^T+{\hat{Q}}_{k-1}---\left(10\right)$

3量测更新

通过乔里斯基分解P_k|k-1：

$P_{k|k-1}=S_{k|k-1}{S}_{k|k-1}^T---\left(11\right)$

计算容积点（i=1,2,…,m；m=2n）：

$X_{i,k|k-1}=S_{k|k-1}{\mathrm{\&xi;}}_i+{\hat{x}}_{k|k-1}---\left(12\right)$

通过量测方程传播容积点：

$z_{i,k|k-1}=h\left(X_{i,k|k-1}\right)---\left(13\right)$

估计k时刻的量测预测值：

${\hat{z}}_{k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}h\left(X_{i,k|k-1}\right)+{\hat{r}}_k---\left(14\right)$

估计自相关协方差阵：

$P_{\mathrm{zz},k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{z}_{i,k|k-1}{z}_{i,k|k-1}^T-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T+{\hat{R}}_k---\left(15\right)$

估计互相关协方差阵：

$P_{\mathrm{xz},k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{X}_{i,k|k-1}{z}_{i,k|k-1}^T-{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T---\left(16\right)$

估计卡尔曼增益矩阵：

$K_k=P_{\mathrm{xz},k|k-1}{P}_{\mathrm{zz},k|k-1}^{-1}---\left(17\right)$

k时刻状态估计值：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})---\left(18\right)$

k时刻状态误差协方差估计值：

$P_{k|k}=P_{k|k-1}-K_k{P}_{\mathrm{zz},k|k-1}{K}_k^T---\left(19\right)$

4构造渐消记忆时变噪声统计估值器

针对容积卡尔曼滤波在噪声统计未知时变情况下易出现滤波精度下降甚至发散的问题，本发明利用MAP估计原理和渐消记忆指数加权方法，设计了一种应用于容积卡尔曼滤波的时变噪声统计估值器，利用输出量测信息实时估计和修正噪声的均值和协方差，从而使容积卡尔曼滤波具有应对噪声统计变化的自适应能力。

（1）构造次优MAP常值噪声统计估值器

已知w_k和v_k服从正态分布，且相互独立，根据MAP估计原理，可得到应用于容积卡尔曼滤波的次优MAP常值噪声统计估值器。下面给出应用于容积卡尔曼滤波的次优MAP常值噪声统计估值器，并参考文献《基于极大后验估计和指数加权的自适应UKF滤波算法》（自动化学报,2010,36(7):1007-1019）和文献《新型的噪声统计估值器和自适应滤波器》（控制理论与应用,1994,11(6):733-737）给出其证明过程。

定理1.基于MAP估计原理和量测值z_k，在假设1和假设2的情况下，次优MAP常值噪声统计估值器如下：

${\hat{q}}_k=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}[{\hat{x}}_{j|j}-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}f\left(X_{i,j-1|j-1}\right)]---\left(20\right)$

${\hat{r}}_k=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}[z_j-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}h\left(X_{i,j|j-1}\right)]---\left(22\right)$

证明：

针对非线性***(5)，根据文献《基于极大后验估计和指数加权的自适应UKF滤波算法》（自动化学报,2010,36(7):1007-1019）中的定理1，可得最优MAP常值噪声统计估值器为：

${\hat{q}}_k=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}[{\hat{x}}_{j|k}-f\left(x_{j-1}\right){|}_{x_{j-1}\&LeftArrow;{\hat{x}}_{j-1|k}}]---\left(24\right)$

${\hat{r}}_k=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}[z_j-h\left(x_j\right){|}_{x_j\&LeftArrow;{\hat{x}}_{j|k}}]---\left(26\right)$

在式(24)～(27)中以滤波估计值

及

或状态预测值来近似代替计算复杂的平滑估计值

及

即可得到次优MAP常值噪声统计估值器：

${\hat{q}}_k=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}[{\hat{x}}_{j|j}-f\left(x_{j-1}\right){|}_{x_{j-1}\&LeftArrow;{\hat{x}}_{j-1|j-1}}]---\left(28\right)$

${\hat{r}}_k=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}[z_j-h\left(x_j\right){|}_{x_j\&LeftArrow;{\hat{x}}_{j|j-1}}]---\left(30\right)$

其中，

的物理意义是：状态估计值

经非线性状态函数f(·)传递之后的后验均值，对于卡尔曼滤波，

可通过线性状态函数传递精确已知，而对于非线性容积卡尔曼滤波，只能以3阶泰勒精度近似已知，其计算公式可借鉴式 ${\hat{x}}_{k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{X}_{i,k|k-1}^{*}=\frac{1}{m}f\left(X_{i,k-1|k-1}\right)+q$ 得到，即：

$f\left(x_{j-1}\right){|}_{x_{j-1}\&LeftArrow;{\hat{x}}_{j-1|j-1}}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}f\left(X_{i,j-1|j-1}\right)---\left(32\right)$

其中，X_i,k-1|k-1为由j-1时刻状态估计值

和协方差P_j-1|j-1构造的容积点。

同理，

的物理意义是：状态预测值

经过非线性量测函数h(·)传递后的后验均值。对于线性KF，

可通过先行量测函数传递精确已知，而对于非线性容积卡尔曼滤波，只能通过3阶泰勒精度近似已知，其计算公式可借鉴式 ${\hat{z}}_{k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{z}_{i,k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}h\left(X_{i,k|k-1}\right)+r$ 得到，即：

$h\left(x_j\right){|}_{x_j\&LeftArrow;{\hat{x}}_{j|j-1}}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}h\left(X_{i,j|j-1}\right)---\left(33\right)$

其中，X_i,k|k-1为由状态预测值

和协方差P_j|j-1构造的的容积点。

将式(32)、(33)代入(28)～(31)，根据状态预测公式

及量测预测公式

可得到应用于容积卡尔曼滤波的次优MAP常值噪声统计估值器(20)～(23)，上述定理1证明完毕。

（2）构造次优无偏MAP常值噪声统计估值器

下述定理2给出应用于容积卡尔曼滤波的次优无偏MAP常值噪声统计估值器。

定理2.基于前述定理1，可得次优无偏MAP常值噪声统计估值器为：

${\hat{q}}_k=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}[(k-1){\hat{q}}_{k-1}+{\hat{x}}_{k|k}-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}f\left(X_{i,k-1|k-1}\right)]---\left(34\right)$

${\hat{Q}}_k=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}[(k-1){\hat{Q}}_{k-1}+K_k{\mathrm{\&gamma;}}_k{\mathrm{\&gamma;}}_k^T{K}_k^T+P_{k|k}-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{X}_{i,k|k-1}{z}_{i,k|k-1}^T+{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T]---\left(35\right)$

${\hat{r}}_k=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}[(k-1){\hat{r}}_{k-1}+z_k-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}h\left(X_{i,k|k-1}\right)]---\left(36\right)$

${\hat{R}}_k=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}[{(k-1){\hat{R}}_{k-1}+\mathrm{\&gamma;}}_k{\mathrm{\&gamma;}}_k^T-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{z}_{i,k|k-1}{z}_{i,k|k-1}^T+{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T]---\left(37\right)$

证明：

对于服从高斯分布的非线性***模型(5)，文献《Stochastic Models,Estimation and Control》（New York:Academic Press,1979）证明了在***理论模型与实际模型完全匹配的前提下，当精确已知***（无论线性或非线性）状态后验均值和协方差时，滤波输出残差序列是零均值高斯白噪声序列。而容积卡尔曼滤波能以至少三阶泰勒精度逼近***输出状态的后验均值和协方差，即容积卡尔曼滤波的输出残差序列近似为零均值高斯白噪声序列，因此有E[γ_k]=0。

根据式 ${\hat{x}}_{k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{X}_{i,k|k-1}^{*}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}f\left(X_{i,k-1|k-1}\right)+q$ 和(20)可得：

$E\left[{\hat{q}}_k\right]=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}E[{\hat{x}}_{j|j}-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}f\left(X_{i,j-1|j-1}\right)]=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}E[{\hat{x}}_{j|j}-({\hat{x}}_{j|j-1}-q)]=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}E[K_j{\mathrm{\&gamma;}}_j+q]=q---\left(38\right)$

由式(38)可知，***噪声均值q的次优MAP常值噪声统计估值器

是无偏的。

根据式 ${\hat{z}}_{k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{z}_{i,k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}h\left(X_{i,k|k-1}\right)+r$ 和(22)可得：

$E\left[{\hat{r}}_k\right]=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}E[z_j-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}h\left(X_{i,j|j-1}\right)]=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}E[z_j-({\hat{z}}_{j|j-1}-r)]=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}E[{\mathrm{\&gamma;}}_j+r]=r---\left(39\right)$

由式(39)可知，量测噪声均值r的次优MAP常值噪声统计估值器

是无偏的。

由式(18)可知， $x_{k|k}-{\hat{x}}_{k|k-1}=K_k{\mathrm{\&gamma;}}_k,$ 由式(19)可知， $P_{k|k-1}-P_{k|k}=K_k{P}_{\mathrm{zz},k|k-1}{K}_k^T,$ 则有：

由式(40)可得：***噪声的协方差矩阵Q的次优无偏MAP常值噪声统计估值器

为：

${\hat{Q}}_k=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}[K_j{\mathrm{\&gamma;}}_j{\mathrm{\&gamma;}}_j^T{K}_j^T+P_{j|j}-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{X}_{i,j|j-1}{z}_{i,j|j-1}^T+{\hat{x}}_{j|j-1}{\hat{z}}_{j|j-1}^T]---\left(41\right)$

残差 ${\mathrm{\&gamma;}}_k=z_k-{\hat{z}}_{k|k-1},$ 且 $P_{\mathrm{zz},k|k-1}=E\left[{\mathrm{\&gamma;}}_k{\mathrm{\&gamma;}}_k^T\right],$ 因此有：

     (42)

由式(42)可得：量测噪声的协方差矩阵R的次优无偏MAP常值噪声统计估值器

为：

${\hat{R}}_k=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}[{\mathrm{\&gamma;}}_j{\mathrm{\&gamma;}}_j^T-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{z}_{i,j|j-1}{z}_{i,j|j-1}^T+{\hat{z}}_{j|j-1}{\hat{z}}_{j|j-1}^T]---\left(43\right)$

根据公式(20)可推导出(34)，根据公式(22)可推导出(36)。根据公式(41)可推导出(35)，根据公式(43)可推导出(37)。定理2证明完毕。

（3）构造渐消记忆时变噪声统计估值器

由公式(34)～(37)可见，对噪声均值和方差的估计是对全部时间点上值的算术平均，和式中每项权系数均为1/k，这不但增加了计算时间，而且耗费存储空间。根据统计学原理，对基于假设3的时变噪声统计量的估计，应强调新近数据的作用，对过于陈旧的数据应逐渐遗忘，因此采用渐消记忆指数加权方法对次优无偏MAP常值噪声统计估值器进行改进，得到基于渐消记忆指数加权方法的时变噪声统计估值器。

假设加权系数{β_i}满足：

$\left\{\begin{array}{cc}{\mathrm{\&beta;}}_i={\mathrm{\&beta;}}_{i-1}b,& 0<b<1\\ \underset{i=1}{\overset{k}{\mathrm{\&Sigma;}}}{\mathrm{\&beta;}}_i=1& \end{array}\right.---\left(44\right)$

其中，b为遗忘因子。

根据等比数列求和公式及加权系数的约束条件，得到加权系数{β_i}与时间相关的一般表达式为：

$\left\{\begin{array}{cc}{\mathrm{\&beta;}}_i=d_k{b}^{i-1},& i=1,...,k\\ d_k=\frac{1-b}{1-b^k}& \end{array}\right.---\left(45\right)$

在和式(34)～(37)中每项乘以β_k+1-j代替原来的权系数1/k，便得到渐消记忆时变噪声统计估值器。以***噪声均值MAP估值器

为例，令：

${\mathrm{\&Theta;}}_{j|j}^q={\hat{x}}_{j|j}-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}f\left(X_{i,j-1|j-1}\right)---\left(46\right)$

即：

${\hat{q}}_k=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}\left[{\mathrm{\&Theta;}}_{j|j}^q\right]---\left(47\right)$

用β_k+1-j代替式(47)中的1/k，并将式(45)与式(47)相乘，可得k时刻渐消记忆时变噪声统计估值器

为：

${\hat{q}}_k=\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}{d}_k{b}^{k-j}{\mathrm{\&Theta;}}_{j|j}^q=d_k\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}{b}^{k-j}{\mathrm{\&Theta;}}_{j|j}^q---\left(48\right)$

同理，k+1时刻渐消记忆时变噪声统计估值器

为：

${\hat{q}}_{k+1}=d_{k+1}\underset{j=1}{\overset{k+1}{\mathrm{\&Sigma;}}}{b}^{k+1-j}{\mathrm{\&Theta;}}_{j|j}^q=d_{k+1}{\mathrm{\&Theta;}}_{k+1|k+1}^q+d_{k+1}\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}{b}^{k+1-j}{\mathrm{\&Theta;}}_{j|j}^q$

$=d_{k-1}{\mathrm{\&Theta;}}_{k+1|k+1}^q+\frac{d_{k+1}b}{d_k}\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}{d}_k{b}^{k-j}{\mathrm{\&Theta;}}_{j|j}^q$

$=d_{k+1}{\mathrm{\&Theta;}}_{k+1|k+1}^q+(1-d_{k+1})\underset{j=1}{\overset{k}{\mathrm{\&Sigma;}}}{d}_k{b}^{k-j}{\mathrm{\&Theta;}}_{j|j}^q---\left(49\right)$

$=(1-d_{k+1}){\hat{q}}_k+d_{k+1}{\mathrm{\&Theta;}}_{k+1|k+1}^q$

$=(1-d_{k+1}){\hat{q}}_k+d_{k+1}[{\hat{x}}_{k+1|k+1}-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}f\left(X_{i,k|k}\right)]$

即：

${\hat{q}}_k=(1-d_k){\hat{q}}_{k-1}+d_k[{\hat{x}}_{k|k}-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}f\left(X_{i,k-1|k-1}\right)]---\left(50\right)$

同理，根据式(41)、(22)、(43)可推导出渐消记忆时变噪声统计估值器

和的递推公式：

${\hat{Q}}_k=(1-d_k){\hat{Q}}_{k-1}+d_k[K_k{\mathrm{\&gamma;}}_k{\mathrm{\&gamma;}}_k^T{K}_k^T+P_{k|k}-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{X}_{i,k|k-1}{z}_{i,k|k-1}^T+{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T]---\left(51\right)$

${\hat{r}}_k=(1-d_k){\hat{r}}_{k-1}+d_k[z_k-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}h\left(X_{i,k|k-1}\right)]---\left(52\right)$

${\hat{R}}_k=(1-d_k){\hat{R}}_{k-1}+d_k[{\mathrm{\&gamma;}}_k{\mathrm{\&gamma;}}_k^T-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{z}_{i,k|k-1}{z}_{i,k|k-1}^T+{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T]---\left(53\right)$

5实时估计和修正噪声

利用公式(50)～(53)对***噪声和量测噪声进行实时估计和修正，重写为：

${\hat{q}}_k=(1-d_k){\hat{q}}_{k-1}+d_k[{\hat{x}}_{k|k}-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}f\left(X_{i,k-1|k-1}\right)]---\left(54\right)$

${\hat{Q}}_k=(1-d_k){\hat{Q}}_{k-1}+d_k[K_k{\mathrm{\&gamma;}}_k{\mathrm{\&gamma;}}_k^T{K}_k^T+P_{k|k}-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{X}_{i,k|k-1}{z}_{i,k|k-1}^T+{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T]---\left(55\right)$

${\hat{r}}_k=(1-d_k){\hat{r}}_{k-1}+d_k[z_k-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}h\left(X_{i,k|k-1}\right)]---\left(56\right)$

${\hat{R}}_k(1-d_k){\hat{R}}_{k-1}+d_k[{\mathrm{\&gamma;}}_k{\mathrm{\&gamma;}}_k^T-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{z}_{i,k|k-1}{z}_{i,k|k-1}^T+{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T]---\left(57\right)$

6分析自适应容积卡尔曼滤波性能

文献《Performance evaluation of UKF-based nonlinear filtering》（Automatica,2006,42(2):261-270）提出了无迹卡尔曼滤波有界收敛定理，接着文献《Comments on“performanceevaluation of UKF-based nonlinear filtering”》（Automatica,2007,43(3):567-568）将该定理扩展到所谓的高斯滤波器中，如扩展卡尔曼滤波，中心差分滤波等。而文献《Author’s reply to‘comments on“performance evaluation of UKF-based nonlinear filtering’”》（Automatica,2007,43(3):569-570）又将该定理扩展到带有非线性量测方程的随机***中。而容积卡尔曼滤波是高斯滤波器，也就是说该定理同样适用于容积卡尔曼滤波，即有下述引理1成立。

引理1.对于非线性***(1)及标准容积卡尔曼滤波，如果有如下假设条件(a)和(b)成立，那么标准容积卡尔曼滤波状态估计误差将是均方有界的，即标准容积卡尔曼滤波稳定收敛。

(a)对

，存在非零实数a_min、a_max、b_max、c_max、g_min和g_max，使得式(58)成立：

$\left\{\begin{array}{cc}{a}_{\min }^{2}I\&le;A_k{A}_k^T\&le;a_{\max }^{2}I,& B_k{B}_k^T\&le;b_{\max }^{2}I\\ g_{\min }^{2}I\&le;G_k{G}_k^T\&le;g_{\max }^{2}I,& C_k{C}_k^T\&le;c_{\max }^{2}I\\ (G_k-B_k){(G_k-B_k)}^T\&le;{(g_{\max }-b_{\max })}^{2}I& \end{array}\right.---\left(58\right)$

(b)存在正实数p_min、p_max、q_max、r_max、

和Σ_min，使得下式成立：

p_minI≤P_k≤p_maxI,Q_k≤q_maxI(59)

$R_k\&le;r_{\max }I,{\mathrm{\&Xi;}}_k\&le;{\mathrm{\&Xi;}}_{\max }I---\left(60\right)$

${\mathrm{\&Xi;}}_k>{\mathrm{\&Xi;}}_{\min }I,{\mathrm{\&Sigma;}}_k>{\mathrm{\&Sigma;}}_{\min }I---\left(61\right)$

其中，

$\left\{\begin{array}{c}{\mathrm{\&Sigma;}}_{\min }=\max \{{\mathrm{\&Sigma;}}_1,{\mathrm{\&Sigma;}}_2\}\\ {\mathrm{\&Sigma;}}_1=a_{\max }^2{(g_{\max }-b_{\max })}^2\&times;(p_{\max }+p_{\max }^2{a}_{\max }^2{\mathrm{\&Xi;}}_{\min }^{-1})\\ {\mathrm{\&Sigma;}}_2=b_{\max }^2(a_{\max }^2{p}_{\max }+{\mathrm{\&Xi;}}_{\max })-g_{\max }^2(a_{\min }^2{p}_{\min }+{\mathrm{\&Xi;}}_{\min })\end{array}\right.---\left(62\right)$

带渐消记忆时变噪声统计估值器的自适应容积卡尔曼滤波中自相关协方差阵分别写为：

$P_{k|k-1}=A_k{P}_{k-1}{A}_k^T+{\mathrm{\&Xi;}}_k^{\&prime;}---\left(63\right)$

$P_{\mathrm{zz},k|k-1}=G_k{P}_{k-1}{G}_k^T+{\mathrm{\&Sigma;}}_k^{\&prime;}---\left(64\right)$

其中，

${\mathrm{\&Xi;}}_k^{\&prime;}=\mathrm{\&delta;}{P}_{k|k-1}+{\hat{Q}}_{k-1}---\left(65\right)$

${\mathrm{\&Sigma;}}_k^{\&prime;}=\mathrm{\&delta;}{P}_{\mathrm{zz},k|k-1}+{\hat{R}}_k---\left(66\right)$

对于带渐消记忆时变噪声统计估值器的自适应容积卡尔曼滤波方法，噪声先验统计虽然未知时变，但是随着噪声统计估值器

和

对先验协方差真实值Q_k-1和R_k的有效跟踪，最终使得

和Σ'_k满足式(61)，从而保证了自适应容积卡尔曼滤波算法的收敛性；且

和

逐渐趋于Q_k-1和R_k，则自适应容积卡尔曼滤波方法估计协方差P_k也愈趋于标准容积卡尔曼滤波协方差极小值P_∞，其在噪声统计未知时变情况下滤波依然收敛。

以下描述本发明的实施例。

为了验证本发明提出的自适应容积卡尔曼滤波方法的有效性，将带有公式(54)～(57)所描述渐消记忆时变噪声统计估值器的自适应容积卡尔曼滤波滤波方法应用于无人水下航行器自主导航***中，采用无人水下航行器海试数据集对自适应容积卡尔曼滤波方法进行验证。

（1）无人水下航行器导航***模型

①无人水下航行器非线性动力学模型

如附图2所示，以无人水下航行器初始位置和初始艏向角

建立全局坐标系L；V是无人水下航行器船体坐标系；E为北东坐标系，North方向为地磁北向。x、y为无人水下航行器在L中的位置；ψ为无人水下航行器在L中的艏向角，显然，其中z_ψ为采用运动传感器测得的无人水下航行器艏向角。

本发明借助式(67)的无人水下航行器四自由度、常速运动模型：

${\left[\begin{array}{c}x\\ y\\ z\\ \mathrm{\&psi;}\\ u\\ v\\ w\\ r\end{array}\right]}_k={\left[\begin{array}{c}x+\mathrm{uT}\cos \left(\mathrm{\&psi;}\right)-\mathrm{vT}\sin \left(\mathrm{\&psi;}\right)\\ y+\mathrm{uT}\sin \left(\mathrm{\&psi;}\right)+\mathrm{vT}\cos \left(\mathrm{\&psi;}\right)\\ z+\mathrm{wT}\\ \mathrm{\&psi;}+\mathrm{rT}\\ u\\ v\\ w\\ r\end{array}\right]}_{k-1}+n_k---\left(67\right)$

式中，[x,y,z,ψ]表示无人水下航行器在L中的位置和艏向；[u,v,w,r]表示无人水下航行器在V中相应的线速度和角速度；k表示任意采样时刻。

②观测模型

无人水下航行器配置了深度计、运动传感器和多普勒流速计程仪。深度计即压力传感器，通过测量水柱压力提供无人水下航行器的深度数据；无人水下航行器通过运动传感器实时测量其艏向角，即无人水下航行器艏艉向与地磁北之间的夹角；多普勒流速计程仪可以测量海流速度、对底跟踪速度等，无人水下航行器在海试中使用多普勒流速计程仪进行对底跟踪速度的测量。它们提供状态向量中深度、艏向和对底速度的直接测量值，因而观测模型为线性的。共同的模型可以写为：

z_k=Hx_k|k-1+s_k     (68)

式中，z_k是观测向量，s_k是观测噪声，测量矩阵H为：

$H=\left[\begin{array}{cccccccc}0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0\end{array}\right].$

③导航***定位误差模型

定义***导航定位误差模型为：

${\mathrm{RMSE}}_{\mathrm{pos}}=\sqrt{\frac{1}{\mathrm{tf}}\underset{k=1}{\overset{\mathrm{tf}}{\mathrm{\&Sigma;}}}({(x_k-{\hat{x}}_k)}^2+{(y_k-{\hat{y}}_k)}^2)}---\left(69\right)$

式中，k是任意时刻，tf是总的运行步数，(x_k,y_k)与分别是无人水下航行器在k时刻的真实位置和滤波算法估计的位置。

（2）试验条件

采用自研无人水下航行器于2010年10月在大连小平岛完成海试，试验中无人水下航行器采集的传感器数据包括深度计、运动传感器、多普勒计程仪等测量数据。无人水下航行器保持近水面航行且采用全球定位***(Global Positioning System，即GPS）记录其航迹。

为了验证本发明提出的自适应容积卡尔曼滤波方法的性能，在***噪声未知的情况下，分别采用无迹卡尔曼滤波、容积卡尔曼滤波和自适应容积卡尔曼滤波方法，基于上述无人水下航行器实测数据集进行试验。

***噪声统计特性未知，设定实际***噪声按定常和时变两种规律变化，进行定常噪声试验和时变噪声试验。表1给出了试验中***噪声的变化规律，t为实验迭代步数，试验总步数为889步。

量测噪声方差为：R=diag([0.03² 0.04² 0.02² 0.01² 0.04²])。

表1***噪声的变化规律

（3）试验结果

附图3～8为***噪声定常和时变两种情况下的试验结果。附图3和附图6为自适应容积卡尔曼滤波、容积卡尔曼滤波和无迹卡尔曼滤波的估计轨迹与GPS轨迹比较，附图4和附图5是东向误差曲线，附图7和附图8是北向误差曲线。

（4）试验结果分析

①突变产生原因

在海试数据集中，无人水下航行器艏向的变化曲线如附图9所示，无人水下航行器的艏向在t=640到t=641之间变化量为-359.6°，导致三种方法在t=641时东向/北向估计误差产生不同程度的突变。

②自适应容积卡尔曼滤波性能分析

东向/北向的估计误差计算公式：ε=E-T，其中，ε、E和T分别为东向/北向的估计误差、估计值和真实值。显然，东向/北向估计误差绝对值|ε|越小，滤波方法精度越高。

根据附图3～8，相比容积卡尔曼滤波和无迹卡尔曼滤波，在定常***噪声和时变***噪声两种情况下，自适应容积卡尔曼滤波的东向估计误差绝对值|ε|较大，但其北向估计误差绝对值|ε|明显小于容积卡尔曼滤波和无迹卡尔曼滤波，估计状态更接近真实状态，估计精度高于容积卡尔曼滤波和无迹卡尔曼滤波。

可见，未知***噪声对滤波性能影响很大，容积卡尔曼滤波和无迹卡尔曼滤波估计过程中明显偏离真实状态，而本发明提出的自适应容积卡尔曼滤波通过对滤波过程中的***噪声进行实时估计，有效解决了由于***噪声统计特性不精确而引起的滤波精度下降问题。

③导航定位误差比较

通过式(69)计算可得，自适应容积卡尔曼滤波、容积卡尔曼滤波和无迹卡尔曼滤波在定常和时变***噪声两种情况下的导航定位误差，如表2所示。可见，无论***噪声定常或时变，自适应容积卡尔曼滤波的导航定位误差都是最小的，无迹卡尔曼滤波的导航定位误差都是最大的。

表2导航定位误差比较

由上述定常和时变***噪声试验结果可得：无论***噪声定常或时变，自适应容积卡尔曼滤波的估计精度最高，无迹卡尔曼滤波的估计精度最低。

本发明所提出自适应容积卡尔曼滤波导航定位精度的提高，使得无人水下航行器无需周期性地上浮至水面进行GPS校正，这对于无人水下航行器执行长航时水下隐蔽监测与作业具有重要的实际应用意义。

Claims (1)

1.一种自适应容积卡尔曼滤波方法，包括如下步骤：

（1）设定初始参数：

设定初始时刻***状态值x₀，初始时刻状态协方差P₀，初始时刻***噪声均值q的渐消记忆时变噪声统计估值器

初始时刻***噪声协方差Q的渐消记忆时变噪声统计估值器

初始时刻***噪声均值r的渐消记忆时变噪声统计估值器

初始时刻***噪声协方差R的渐消记忆时变噪声统计估值器

遗忘因子b；

（2）时间更新：

进行自适应容积卡尔曼滤波基于标准容积卡尔曼滤波的时间更新：

$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T,$

$X_{i,k-1|k-1}=S_{k-1|k-1}{\mathrm{\&xi;}}_i+{\hat{x}}_{k-1|k-1},$

$X_{i,k-1|k-1}^{*}=f\left(X_{i,k-1|k-1}\right),$

在k时刻的状态预测值

中融入***噪声均值q的渐消记忆时变噪声统计估值器

${\hat{x}}_{k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}f\left(X_{i,k-1|k-1}\right)+{\hat{q}}_{k-1},$

在k时刻的误差协方差预测值P_k|k-1中融入***噪声协方差差Q的渐消记忆时变噪声统计估值器

$P_{k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{X}_{i,k|k-1}^{*}{X}_{i,k|k-1}^{*T}-{\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}^T+{\hat{Q}}_{k-1},$

其中，P_k-1|k-1为k-1时刻的误差协方差；S_k-1|k-1为P_k-1|k-1乔里斯基因子；X_i,k-1|k-1为容积点集；m=2n，n为状态向量的维数；

记n维单位列向量e=[1,0,…，0]^T，使用符号[1]表示对e的元素进行全排列和改变元素符号产生的点集，称为完整全对称点集，[1]_i表示点集[1]中的第i个点；

为k-1时刻的状态估计值；

为通过状态方程传播的容积点集；f(□)为非线性状态方程；

为k时刻的状态预测值；P_k|k-1为k时刻的状态误差协方差预测值；

为***噪声均值q的渐消记忆时变噪声统计估值器；

为***噪声协方差Q的渐消记忆时变噪声统计估值器；

（3）量测更新：

进行自适应容积卡尔曼滤波基于标准容积卡尔曼滤波的量测更新：

$P_{k|k-1}=S_{k|k-1}{S}_{k|k-1}^T,$

$X_{i,k-1|k-1}=S_{k-1|k-1}{\mathrm{\&xi;}}_i+{\hat{x}}_{k-1|k-1},$

z_i,k|k-1=h(X_i,k|k-1)，

在k时刻的量测预测值

中融入量测噪声均值r的渐消记忆时变噪声统计估值器

${\hat{z}}_{k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}h\left(X_{i,k|k-1}\right)+{\hat{r}}_k,$

在自相关协方差矩阵P_zz,k|k-1中融入量测噪声协方差R的渐消记忆时变噪声统计估值器

$P_{\mathrm{zz},k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{z}_{i,k|k-1}{z}_{i,k|k-1}^T-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T+{\hat{R}}_k,$

结合标准容积卡尔曼滤波中互相关协方差矩阵P_xz,k|k-1，卡尔曼增益K_k，k时刻的状态估计值k时刻的状态误差协方差估计值P_k|k的完成量测更新：

$P_{\mathrm{xz},k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{X}_{i,k|k-1}{z}_{i,k|k-1}^T-{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T,$

$K_k=P_{\mathrm{xz},k|k-1}{P}_{\mathrm{zz},k|k-1}^{-1},$

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1}),$

$P_{k|k}=P_{k|k-1}-K_k{P}_{\mathrm{zz},k|k-1}{K}_k^T,$

其中，S_k|k-1为P_k|k-1乔里斯基因子；X_i,k|k-1为容积点集；z_i,k|k-1为通过量测方程传播的容积点集；h(□)为非线性量测函数；

为k时刻的量测预测值；

为量测噪声均值r的渐消记忆时变噪声统计估值器；P_zz,k|k-1为自相关协方差矩阵；

为量测噪声协方差R的渐消记忆时变噪声统计估值器；P_xz,k|k-1为互相关协方差矩阵；K_k为卡尔曼增益；

为k时刻的状态估计值；z_k为k时刻的量测值；P_k|k为k时刻的状态误差协方差估计值；

（4）构造渐消记忆时变噪声统计估值器：

构造次优MAP常值噪声统计估值器；

分析次优MAP常值噪声统计估值器的无偏性，构造次优无偏MAP常值噪声统计估值器；

在次优无偏MAP常值噪声统计估值器的基础上，借助渐消记忆指数加权方法，构造的渐消记忆时变噪声统计估值器

和

${\hat{q}}_k=(1-d_k){\hat{q}}_{k-1}+d_k[{\hat{x}}_{k|k}-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}f\left(X_{i,k-1|k-1}\right)],$

${\hat{Q}}_k=\left({1-d}_k\right){\hat{Q}}_{k-1}+d_k[K_k{\mathrm{\&gamma;}}_k{\mathrm{\&gamma;}}_k^T{K}_k^T+P_{k|k}-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{X}_{i,k|k-1}{z}_{i,k|k-1}^T+x_{k|k-1}^{^}{\hat{z}}_{k|k-1}^T],$

${\hat{r}}_k=(1-d_k){\hat{r}}_{k-1}+d_k[z_k-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}h\left(X_{i,k|k-1}\right)],$

${\hat{R}}_k=(1-d_k){\hat{R}}_{k-1}+d_k[{\mathrm{\&gamma;}}_k{\mathrm{\&gamma;}}_k^T-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{z}_{i,k|k-1}{z}_{i,k|k-1}^T+{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T],$

其中，d_k=(1-b)/(1-b^k)，b为遗忘因子，且满足0<b<1；γ_k为k时刻的残差；

（5）实时估计和修正噪声：

利用步骤（4）中所描述的渐消记忆时变噪声统计估值器和

对***噪声Q和量测噪声R进行实时估计和修正。

Images