CN103281054A - 一种带噪声统计估值器的自适应滤波方法 - Google Patents

Abstract

本发明公开了一种带噪声统计估值器的自适应滤波方法，目的是解决在系统噪声和量测噪声的统计信息未知或者时变情况下，常规卡尔曼滤波精度下降甚至发散的问题。针对噪声统计特性未知或者时变的情况，首先基于极大后验估计理论推导次优极大后验噪声统计估值器，然后利用一步最优平滑器得到改进的卡尔曼滤波器，最后经过无偏性检验得到无偏次优极大后验噪声统计估值器。在SINS/GPS组合导航系统中进行的仿真结果表明：本发明设计的自适应滤波器不仅可以准确的估计出噪声的统计信息，而且显著提高了滤波精度，并改善了系统鲁棒性。

Description

一种带噪声统计估值器的自适应滤波方法

技术领域

本发明属于导航系统技术领域，涉及一种自适应卡尔曼滤波器的在线设计方法，特别是在系统噪声和量测噪声统计特性未知或者时变的情况下，可有效提高滤波器的滤波精度和鲁棒性的自适应滤波方法。

背景技术

在捷联惯导与卫星导航(SINS/GPS)组合导航系统中，卡尔曼滤波算法可以估计系统的平台失准角以及陀螺的常值漂移等状态变量，在系统的初始对准和导航定位中发挥重要作用，因此在组合导航及其它状态估计问题中获得了广泛的应用。

常规卡尔曼滤波技术在工作时需要满足模型精确和噪声统计特性准确已知的要求，然而在SINS/GPS组合导航系统中不可避免的含有不确定性噪声，例如陀螺随机漂移、随机风速以及海流等干扰因素。此时常规卡尔曼滤波将失去最优估计特性，滤波精度将大大降低，甚至导致滤波发散。

为解决噪声统计信息未知或者时变带来的滤波精度下降的问题，近年来学者们提出了多种自适应滤波算法，主要集中在基于极大似然估计的新息自适应卡尔曼滤波算法和基于极大后验估计的Sage-Husa自适应滤波算法。文献“组合导航系统新息自适应卡尔曼滤波算法，上海交通大学学报，2006，第40卷第6期”提出的基于极大似然估计的新息自适应滤波算法需要依据经验得到相关初始信息，并受到平滑窗口宽度的限制，难以保证较高的滤波精度；文献“自适应滤波方法研究，航空学报，1998，第19卷第7期”研究认为Sage-Husa自适应滤波算法对极大后验噪声估值器的简化导致系统的鲁棒性降低，使得滤波器容易失去正定性而发散；本发明所设计的一种自适应卡尔曼滤波器的在线设计方法，通过引入一步最优平滑器和次优极大后验噪声统计估值器，能够对噪声的统计特性进行在线实时估计，提高了系统的滤波精度和鲁棒性，具有实际的工程应用意义。

发明内容

为了克服现有技术中的缺陷，解决上述技术问题，本发明提供一种带噪声统计估值器的自适应滤波方法，通过在线加入噪声统计估值器，在系统噪声和量测噪声统计信息未知或者时变的情况下，对其统计特性进行实时估计与修正，使得滤波器能根据噪声变化自适应的调节增益矩阵，从而提高滤波器精度和鲁棒性的自适应滤波方法。

其技术方案如下：

一种带噪声统计估值器的自适应滤波方法，包括以下步骤：

步骤一：通过对连续系统离散化，建立离散系统的状态方程和量测方程；

步骤二：建立一步最优平滑器，利用k+1时刻的量测信息Z_k+1对k时刻估计值

进行一步平滑修正；

步骤三：用k时刻一步最优平滑值

代替k时刻滤波估计值

并对k+1时刻的离散系统进行滤波；

步骤四：基于极大后验噪声估值器，用一步最优平滑值

近似代替

以滤波估计值

代替

建立次优极大后验噪声统计估值器；

步骤五：对k+1时刻得到的噪声统计估值器进行无偏估计检验，建立无偏的次优极大后验噪声统计估值器；

步骤六：根据系统状态方程，并结合步骤二到步骤五，依次利用一步最优平滑器、改进的卡尔曼滤波器以及无偏极大后验噪声统计估值器完成对系统的自适应卡尔曼滤波递推运算。

进一步优选，步骤三中借助k+1时刻的量测信息Z_k+1对k时刻估计值

进行一步平滑修正，并利用k时刻的一步最优平滑值

代替滤波估计值

对k+1时刻离散系统进行滤波，具体实现为：

${\hat{X}}_{k+1,k}={\mathrm{\Φ}}_k{\hat{X}}_{k,k+1}+{\mathrm{\Γ}}_k{q}_k$

式中，

是系统k+1时刻的一步预测估计值，

是k时刻的一步最优平滑值，Φ_k是k时刻的一步转移矩阵，Γ_k是k时刻的系统噪声系数阵，

是k时刻的系统噪声估计均值。

进一步优选，步骤四中用一步最优平滑值

和滤波估计值

分别近似代替复杂的多步平滑值

和得到次优极大后验估值器，具体实现为：

${\mathrm{\Γ}}_{k+1}{\hat{q}}_{k+1}=\frac{1}{k+1}\underset{j=1}{\overset{k+1}{\mathrm{\Σ}}}[{\hat{X}}_{j-1}-{\mathrm{\Φ}}_{j-1}{\hat{X}}_{j-1,j}]$

${\mathrm{\Γ}}_{k+1}{\hat{Q}}_{k+1}{\mathrm{\Γ}}_{k+1}^T=\frac{1}{k+1}\underset{j=1}{\overset{k+1}{\mathrm{\Σ}}}[{\hat{X}}_{j,j}-{\mathrm{\Φ}}_{j-1}{\hat{X}}_{j-1,j}-\mathrm{\Γq}]{[{\hat{X}}_{j,j}-{\mathrm{\Φ}}_{j-1}{\hat{X}}_{j-1,j}-\mathrm{\Γq}]}^T$

${\hat{r}}_{k+1}=\frac{1}{k+1}\underset{j=1}{\overset{k+1}{\mathrm{\Σ}}}[Z_j-H_j{\hat{X}}_{j,j}]$

${\hat{R}}_{k+1}=\frac{1}{k+1}\underset{j=1}{\overset{k+1}{\mathrm{\Σ}}}[Z_j-H_j{\hat{X}}_{j,j}-r{\left]\right[Z_j-H_j{\hat{X}}_{j,j}-r]}^T$

式中，

和

分别是k+1时刻的系统噪声的估计均值和协方差；和分别是k+1时刻的量测噪声的估计均值和协方差。

本发明的有益效果：

(1)本发明通过在线加入无偏噪声统计估值器对未知或时变的噪声进行实时估计修正，并利用一步最优平滑器改进卡尔曼滤波器，不仅可以为卡尔曼滤波器提供实时准确的噪声统计信息，使得滤波器能根据噪声变化自适应的调节增益矩阵，提高滤波的精度；而且提高了系统对初始误差的抗干扰能力，改善了滤波器对系统噪声和量测噪声不确定性扰动的鲁棒性，明显提高了滤波器的收敛速度。

(2)本发明应用于SINS/GPS组合导航系统中，实现了对系统噪声和量测噪声的实时准确估计，滤波器增益矩阵能够根据噪声统计信息的变化进行自适应调节，有效提高了组合导航系统的导航定位精度。

(3)本发明的有益效果可以通过Matlab仿真实验进行验证，设置了与常规卡尔曼滤波在SINS/GPS组合导航系统的对比仿真。在SINS/GPS组合导航系统中仿真参数设置如表1所示：

表1仿真参数设置表

设噪声方差理论值为Q和R，假定对系统噪声统计信息未知，取系统噪声方差为10Q，量测噪声为0.01R；计算步长设为0.1s，仿真时间1000s；初始位置姿态：经度为126.67°，纬度为45.77°，姿态角(0，0，5°)。

通过在SINS/GPS组合导航系统的对比仿真可以看出，在噪声统计信息未知或者不准确的情况下，与常规卡尔曼滤波相比，本发明提出的自适应滤波器可以实时根据准确估计噪声的统计特性(见图6)，不仅使得滤波精度明显提高，而且使得滤波器的收敛速度加快，鲁棒性得到显著提高(见图5)；同时本发明提出的自适应滤波器进一步提高了导航系统的定位精度，具有更加广阔的工程应用前景。

附图说明

图1是本发明自适应滤波器的设计流程图；

图2是本发明自适应滤波器在SINS/GPS组合导航系统中的工作方框图；

图3是本发明自适应滤波器与常规卡尔曼滤波的关于航向误差角仿真对比图；

图4是本发明自适应滤波器与常规卡尔曼滤波的关于纬度误差仿真对比图；

图5是本发明自适应滤波器与常规卡尔曼滤波的关于东向速度误差仿真对比图；

图6是本发明自适应滤波器对量测噪声统计协方差的估计值仿真图。

具体实施方式

下面结合附图和具体实施例对本发明的技术方案作进一步详细地说明。

参照图1-图2，一种带噪声统计估值器的自适应滤波方法，包括以下步骤：

步骤一：建立SINS/GPS组合导航系统状态空间方程。在SINS/GPS组合导航模式下，SINS作为主导航系统建立系统的状态方程，并结合捷联惯导系统的误差方程，选取经纬度误差、速度误差以及失准角作为状态变量；GPS作为辅助导航系统提供外速度信息作为量测值。

状态变量为

状态方程：X_k＝Φ_kX_k-1+Γ_kW_k

量测方程：Z_k＝H_kX_k+V_k

其中，状态一步转移矩阵

${\mathrm{\Φ}}_k=$

$=\left[\begin{array}{ccccccc}1& 0& \frac{T}{R}& 0& 0& 0& 0\\ \frac{v_E}{R}T\sec L\tan L& 1& \frac{\sec L}{R}T& 0& 0& 0& 0\\ {2w}_{\mathrm{ie}}T\cos L{v}_N+\frac{v_E{v}_N}{R}{T\sec }^{2}L& 0& 1+\frac{v_N}{R}T\tan L& {2w}_{\mathrm{ie}}\sin L+\frac{v_E}{R}\tan L& 0& -\mathrm{gT}& f_{N}T\\ -({2w}_{\mathrm{ie}}T\cos L{v}_E+\frac{v_E^2}{R}{T\sec }^{2}L)& 0& -({2w}_{\mathrm{ie}}T\sin L+\frac{v_E}{R}T\tan L)& 1& \mathrm{gT}& 0& -f_{E}T\\ 0& 0& 0& -\frac{T}{R}& 1& w_{\mathrm{ie}}+\frac{v_E}{R}\tan L& -(w_{\mathrm{ie}}T\cos L+\frac{v_{E}T}{R})\\ {-w}_{\mathrm{ie}}T\sin L& 0& \frac{T}{R}& 0& -(w_{\mathrm{ie}}T\sin L+\frac{v_E}{R}T\tan L)& 1& \frac{v_N}{R}T\\ w_{\mathrm{ie}}T\cos L+\frac{v_E}{R}{T\sec }^{2}L& 0& \frac{T}{R}\tan L& 0& w_{\mathrm{ie}}T\cos L+\frac{v_E}{R}T& \frac{v_N}{R}T& 1\end{array}\right]$

系统噪声系数阵

${\mathrm{\Γ}}_k=$

$=T\left[\begin{array}{ccccccc}0& 0& 0& 0& 0& 0& 0\\ 0& 0& & 0& 0& 0& 0\\ 0& 0& C_b^n\left(\mathrm{1,1}\right)& C_b^n\left(\mathrm{1,2}\right)& 0& 0& 0\\ 0& 0& C_b^n\left(\mathrm{2,1}\right)& C_b^n\left(\mathrm{2,2}\right)& 0& 0& 0\\ 0& 0& 0& 0& C_b^n\left(\mathrm{1,1}\right)& C_b^n(1,2)& C_b^n\left(\mathrm{1,3}\right)\\ 0& 0& 0& 0& C_b^n\left(\mathrm{2,1}\right)& C_b^n\left(\mathrm{2,2}\right)& C_b^n\left(\mathrm{2,3}\right)\\ 0& 0& 0& 0& C_b^n\left(\mathrm{3,1}\right)& C_b^n\left(\mathrm{3,2}\right)& C_b^n\left(\mathrm{3,3}\right)\end{array}\right]$

量测系数阵

$H_k=\left[\begin{array}{ccccccc}0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0\end{array}\right]$

步骤二：建立SINS/GPS组合导航系统的一步最优平滑器，利用k+1时刻的量测信息Z_k+1对k时刻估计值

进行一步平滑修正；

$X_{k+1,k}={\mathrm{\Φ}}_k{\hat{X}}_k+{\mathrm{\Γ}}_k{q}_k$

ε_k+1＝Z_k+1-H_k+1X_k+1，k-r_k+1

P_k+1，k＝Φ_kP_kΦ^T _k+Γ_kQ_kΓ^T _k

$M_{k+1,k}=P_k{\mathrm{\Φ}}_k^T{H}_{k+1}^T{(H_{k+1}{P}_{k+1,k}{H}_{k+1}^T+R_{k+1})}^{-1}$

${\hat{X}}_{k,k+1}={\hat{X}}_k+M_{k+1}{\mathrm{\ϵ}}_{k+1}$

式中，M_k+1是一步平滑增益矩阵，P_k是状态估计方差阵，P_k+1，k是一步预测方差阵，R_k+1是量测噪声方差阵，

是k时刻的一步最优平滑值，ε_k+1是k+1时刻的量测信息。

步骤三：用k时刻的一步最优平滑值

代替估计值

得到改进的卡尔曼滤波器，并对k+1时刻的组合导航系统进行滤波：

${\hat{X}}_{k+1,k}={\mathrm{\Φ}}_k{\hat{X}}_{k,k+1}+{\mathrm{\Γ}}_k{q}_k$

${\mathrm{\ϵ}}_{k+1}=Z_{k+1}-H_{k+1}{\hat{X}}_{k+1,k}-r_{k+1}$

P_k+1，k＝Φ_kP_kΦ^T _k+Γ_kQ_kΓ^T _k

$K_{k+1}=P_{k+1,k}{H}_{k+1}^T{(H_{k+1}{P}_{k+1,k}{H}_{k+1}^T+R_{k+1})}^{-1}$

${\hat{X}}_{k+1}={\hat{X}}_{k+1,k}+K_{k+1}{\mathrm{\ϵ}}_{k+1}$

P_k+1＝(I-K_k+1H_k+1)P_k+1，k

式中，

是系统噪声估计均值，

是量测噪声估计均值，Q_k是系统噪声方差阵，K_k+1是滤波增益矩阵。

步骤四：基于极大后验估计理论，得到极大后验噪声统计估值器。利用一步平滑估值

近似代替

以滤波估计值

代替

对极大后验估值器进行简化，建立次优极大后验噪声统计估值器：

${\mathrm{\Γ}}_{k+1}{\hat{q}}_{k+1}=\frac{1}{k+1}\underset{j=1}{\overset{k+1}{\mathrm{\Σ}}}\left[{\hat{X}}_{j-1}{-{\mathrm{\Φ}}_{j-1}\hat{X}}_{j-1,j}\right]$

${\mathrm{\Γ}}_{k+1}{\hat{Q}}_{k+1}{\mathrm{\Γ}}_{k+1}^T=\frac{1}{k+1}\underset{j=1}{\overset{k+1}{\mathrm{\Σ}}}[{\hat{X}}_{j,j}-{\mathrm{\Φ}}_{j-1}{\hat{X}}_{j-1,j}-\mathrm{\Γq}]{[{\hat{X}}_{j,j}-{\mathrm{\Φ}}_{j-1}{\hat{X}}_{j-1,j}-\mathrm{\Γq}]}^T$

${\hat{r}}_{k+1}=\frac{1}{k+1}\underset{j=1}{\overset{k+1}{\mathrm{\Σ}}}[Z_j-H_j{\hat{X}}_{j,j}]$

${\hat{R}}_{k+1}=\frac{1}{k+1}\underset{j=1}{\overset{k+1}{\mathrm{\Σ}}}[Z_j-H_j{\hat{X}}_{j,j}-r{\left]\right[Z_j-H_j{\hat{X}}_{j,j}-r]}^T$

步骤五：对由步骤四得到k+1时刻得到的噪声统计估值器进行无偏估计检验，建立无偏的极大后验噪声统计估值器，其递推形式：

${\mathrm{\Γ}}_{k+1}{\hat{q}}_{k+1}=+{\mathrm{\Γ}}_k{\hat{q}}_k+\frac{1}{k+1}\left[K_{k+1}{\mathrm{\ϵ}}_{k+1}\right]$

${\mathrm{\Γ}}_{k+1}{\hat{Q}}_{k+1}{\mathrm{\Γ}}_{k+1}^T=(1-\frac{1}{k+1}){\mathrm{\Γ}}_k{\hat{Q}}_k{\mathrm{\Γ}}_k^T+$

$\frac{1}{k+1}[K_{k+1}{\mathrm{\ϵ}}_{k+1}{\mathrm{\ϵ}}_{k+1}^T{K}_{k+1}^T+P_{k+1}{-\mathrm{\Φ}}_k{P}_k{{\mathrm{\Φ}}^T}_k]$

${\hat{r}}_{k+1}={\hat{r}}_k+\frac{1}{k+1}\left[\left({I-H}_{k+1}{K}_{k+1}\right){\mathrm{\ϵ}}_{k+1}\right]$

${\hat{R}}_{k+1}=(1-\frac{1}{k+1}){\hat{R}}_k+$

$\frac{1}{k+1}\left[\left(I{-H}_{k+1}{K}_{k+1}\right)\right[{\mathrm{\ϵ}}_{k+1}{\mathrm{\ϵ}}_{k+2}^T{\left({I-H}_{k+1}{K}_{k+1}\right)}^T+H_{k+1}{P}_{k+1,k}{H}_{k+1}^T]$

步骤六：根据SINS/GPS组合导航系统方程，并结合步骤二到步骤五，依次利用一步最优平滑器、改进的卡尔曼滤波器以及无偏极大后验噪声统计估值器完成对系统的自适应卡尔曼滤波递推运算。

以上所述，仅为本发明较佳的具体实施方式，本发明的保护范围不限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可显而易见地得到的技术方案的简单变化或等效替换均落入本发明的保护范围内。

Claims (3)

1.一种带噪声统计估值器的自适应滤波方法，其特征在于，包括以下步骤：

步骤一：通过对连续系统离散化，建立离散系统的状态方程和量测方程；

步骤二：建立一步最优平滑器，利用k+1时刻的量测信息Z_k+1对k时刻估计值

进行一步平滑修正；

步骤三：用k时刻一步最优平滑值

代替k时刻滤波估计值

并对k+1时刻的离散系统进行滤波；

步骤四：基于极大后验噪声估值器，用一步最优平滑值

近似代替

以滤波估计值

代替

建立次优极大后验噪声统计估值器；

步骤五：对k+1时刻得到的噪声统计估值器进行无偏估计检验，建立无偏的次优极大后验噪声统计估值器；

步骤六：根据系统状态方程，并结合步骤二到步骤五，依次利用一步最优平滑器、改进的卡尔曼滤波器以及无偏极大后验噪声统计估值器完成对系统的自适应卡尔曼滤波递推运算。

2.根据权利要求1所述的带噪声统计估值器的自适应滤波方法，其特征在于，步骤三中借助k+1时刻的量测信息Z_k+1对k时刻估计值进行一步平滑修正，并利用k时刻的一步最优平滑值

代替滤波估计值

对k+1时刻离散系统进行滤波，具体实现为：

${\hat{X}}_{k+1,k}={\mathrm{\Φ}}_k{\hat{X}}_{k,k+1}+{\mathrm{\Γ}}_k{q}_k$

式中，

是系统k+1时刻的一步预测估计值，是k时刻的一步最优平滑值，Φ_k是k时刻的一步转移矩阵，Γ_k是k时刻的系统噪声系数阵，

是k时刻的系统噪声估计均值。

3.根据权利要求1所述的带噪声统计估值器的自适应滤波方法，其特征在于，步骤四中用一步最优平滑值

和滤波估计值分别近似代替复杂的多步平滑值

和

得到次优极大后验估值器，具体实现为：

${\mathrm{\Γ}}_{k+1}{\hat{q}}_{k+1}=\frac{1}{k+1}\underset{j=1}{\overset{k+1}{\mathrm{\Σ}}}\left[{\hat{X}}_{j,j}{{-\mathrm{\Φ}}_{j-1}\hat{X}}_{j-1,j}\right)]$

${\mathrm{\Γ}}_{k+1}{\hat{Q}}_{k+1}{\mathrm{\Γ}}_{k+1}^T=\frac{1}{k+1}\underset{j=1}{\overset{k+1}{\mathrm{\Σ}}}[{\hat{X}}_{j,j}-{\mathrm{\Φ}}_{j-1}{\hat{X}}_{j-1,j}-\mathrm{\Γq}]{[{\hat{X}}_{j,j}-{\mathrm{\Φ}}_{j-1}{\hat{X}}_{j-1,j}-\mathrm{\Γq}]}^T$

${\hat{r}}_{k+1}=\frac{1}{k+1}\underset{j=1}{\overset{k+1}{\mathrm{\Σ}}}[Z_j-H_j{\hat{X}}_{j,j}]$

${\hat{R}}_{k+1}=\frac{1}{k+1}\underset{j=1}{\overset{k+1}{\mathrm{\Σ}}}[Z_j-H_j{\hat{X}}_{j,j}-r{\left]\right[Z_j-H_j{\hat{X}}_{j,j}-r]}^T$

式中，

和

分别是k+1时刻的系统噪声的估计均值和协方差；

和

分别是k+1时刻的量测噪声的估计均值和协方差。

Images