CN101853243A - 系统模型未知的自适应卡尔曼滤波方法 - Google Patents

Abstract

本发明公开了一种系统模型未知的自适应卡尔曼滤波方法，其目的是解决系统状态模型未知或者时变的情况下，卡尔曼滤波方法滤波精度下降甚至发散的问题。针对系统状态模型建立的未知或时变情况，在卡尔曼滤波的基础上采用信息计算滤波残差，根据滤波残差和观测噪声协方差的比值计算出一个加权系数，通过此加权系数对系统状态噪声协方差阵进行实时修正，有效地解决了由于系统模型未知或随时间变化导致的卡尔曼滤波精度下降甚至发散的问题。在同等条件下，X轴滤波误差均值由现有技术的0.1231下降到0.0056；Y轴滤波误差均值由现有技术的0.3895下降到0.0039。

Description

系统模型未知的自适应卡尔曼滤波方法

技术领域

本发明涉及一种自适应卡尔曼滤波方法，特别是系统状态模型未知或时变的自适应卡尔曼滤波方法。

背景技术

卡尔曼滤波一般在进行滤波时，需要精确获知系统的状态模型和系统噪声的统计特性。然而在实际情况中，系统状态模型往往是未知的或者时变的，例如，飞机在空中的运动状态是时变且难以预测的。这种由于系统的状态模型的未知或时变性会使得卡尔曼滤波精度降低甚至引起发散。

解决系统状态模型不确定或时变的方法是采用自适应卡尔曼滤波算法。近年来，许多学者对自适应卡尔曼滤波算法进行了深入和广泛的研究，常用的自适应卡尔曼滤波算法是Sage-Husa自适应算法。文献“简化的Sage-Husa自适应滤波算法在组合导航中的应用及仿真，青岛大学学报，2001，16(1)”中公开了一种简化的Sage-Husa自适应卡尔曼滤波算法。该方法通过在线调节系统的状态噪声或观测噪声的协方差阵来解决系统状态模型的时变性和随机性。但是这种方法适用于系统状态模型已知、状态噪声未知的情况，对于系统状态模型未知或者随时间变化的情况其滤波精度较差。例如，在下述不确切状态下，

$\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·\·}{x}\\ \stackrel{\·}{y}\\ \stackrel{\·\·}{y}\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 0& 0& 0\end{array}\right]\·\left[\begin{array}{c}x\\ \stackrel{\·}{x}\\ y\\ \stackrel{\·}{y}\end{array}\right]+\left[\begin{array}{c}0\\ w_1\\ 0\\ w_2\end{array}\right]$

X轴滤波误差均值是0.1231；Y轴滤波误差均值是0.3895。

发明内容

为了克服现有技术自适应卡尔曼滤波方法针对系统状态模型未知或者时变的情况，滤波精度下降甚至发散的不足，本发明提供一种系统模型未知的自适应卡尔曼滤波方法。该方法通过对比滤波残差与观测噪声之间的关系，计算出一个时变加权修正系数，从而实时调节状态噪声的协方差阵，可以提高滤波精度。

本发明解决其技术问题所采用的技术方案：一种系统模型未知的自适应卡尔曼滤波方法，其特点是包括下述步骤：

(a)建立系统离散化和线性化后的状态方程：

x(k+1)＝Φx(k)+w(k)

式中，x为m维状态向量，Φ为m×m维状态转移矩阵，w为m维系统状态噪声，

建立系统离散化和线性化后的观测方程：

z(k)＝Hx(k)+v(k)

式中，z为n维观测向量；H是n×m维观测矩阵，v为n维系统状态噪声；

(b)采用卡尔曼滤波方法，对第k+1时刻进行滤波；

$\hat{x}(k+1/k)=\mathrm{\Φ}(k+1,k)\hat{x}(k/k)$

$P(k+1/k)=\mathrm{\Φ}(k+1,k)P(k/k){\mathrm{\Φ}}^T(k+1,k)+{\hat{Q}}_k$

K(k+1)＝P(k+1/k)H^T(k+1)[H(k+1)P(k+1/k)H^T(k+1)+R_k+1]^-1

$\hat{x}(k+1/k+1)=\hat{x}(k+1/k)+K(k+1)[z(k+1)-H(k+1)\hat{x}(k+1/k)]$

P(k+1/k+1)＝P(k+1/k)-P(k+1/k)H^T(k+1)

·[H(k+1)P(k+1/k)H^T(k+1)+R_k+1]^-1H(k+1)P(k+1/k)

(c)滤波残差是，

${\mathrm{\ϵ}}_{k+1}=z_{k+1}-H{\hat{x}}_{k+1/k+1}$

式中，ε_k+1是滤波残差，z_k+1是第k+1时刻的观测值，H是系统观测矩阵，

是第k+1时刻状态量的估计值；

(d)修正加权系数是，

$g_i=\frac{{\mathrm{\ϵ}}_{i(k+1)}}{R_i}$

r＝[R₁ R₂…R_n]^T

k＝H′·(g+r)

式中，i＝1～n，R_i为观测噪声协方差阵R对角线上第i个数，k为修正加权系数，为m维向量；

(e)修正系统噪声协方差阵，

${\hat{q}}_j=k_j\·q_j$

${\hat{Q}}_{k+1}=\mathrm{diag}\left[\begin{array}{cccc}{\hat{q}}_1& {\hat{q}}_2& ...& {\hat{q}}_m\end{array}\right]$

式中，j＝1～m，

是估计出的第k+1时刻系统噪声协方差阵，

是修正后状态噪声协方差矩阵

中对角线上第j个分量，q_j是修正前系统噪声协方差阵Q_k+1对角线上第j个分量。

本发明的有益效果是：由于通过对比滤波残差与观测噪声之间的关系，并将该残差值引入到系统状态噪声协方差阵的估计中来，计算出加权系数，对系统状态噪声协方差阵进行实时的修正，有效地解决了由于系统模型未知或随时间变化导致的卡尔曼滤波精度下降甚至发散的问题。在同等条件下，X轴滤波误差均值由现有技术的0.1231下降到0.0056；Y轴滤波误差均值由现有技术的0.3895下降到0.0039。

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

附图说明

图1是本发明系统模型未知的自适应卡尔曼滤波方法的流程图。

图2是目标实际机动曲线图。

图3是采用背景技术简化的Sage-Husa自适应滤波方法估计状态结果曲线图。

图4是采用本发明系统模型未知的自适应卡尔曼滤波方法估计状态结果曲线图。

具体实施方式

参照图1～4。以机动目标的跟踪为例，本发明方法的具体步骤如下：

步骤1：建立目标的运动状态方程，假设目标的真实运动轨迹为x方向和y方向做衰减正弦运动，且x方向的振荡频率为变频率。由于不能事先知道目标的运动情况，所以在建立状态方程时，不可能精确描述其运动轨迹，假设其按匀速直线运动，则建立的系统状态方程如下式所示：

$\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·\·}{x}\\ \stackrel{\·}{y}\\ \stackrel{\·\·}{y}\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 0& 0& 0\end{array}\right]\·\left[\begin{array}{c}x\\ \stackrel{\·}{x}\\ y\\ \stackrel{\·}{y}\end{array}\right]+\left[\begin{array}{c}0\\ w_1\\ 0\\ w_2\end{array}\right]$

其中，状态量x，

为目标x轴方向的位置和速度，状态量y，

为目标y轴方向的位置和速度，w₁，w₂为系统状态噪声。

将状态方程写成向量的形式，如下所示：

$\stackrel{\·}{x}=\mathrm{Ax}+w$

其中，

为系统的状态量，为4维向量，w系统状态噪声向量，

为状态矩阵。

滤波时，采用的方程为离散化后的状态方程，假设滤波周期为T，则离散化后的状态方程为：

x(k+1)＝(I+T·A)x(k)+T·w

＝Φx(k)+w(k)

建立系统的观测方程，系统观测量为雷达探测的距离和观测角：

$\mathrm{\ρ}=\sqrt{x^2+y^2}+v_1$

$\mathrm{\α}=\arctan \frac{y}{x}+v_2$

其中，ρ为雷达探测的距离，α为雷达的观测角。

将观测方程写成向量的形式，如下所示：

z＝h(x)+v

其中，z＝[ρα]^T为观测向量，为2维向量，h(·)为观测方程函数，v观测噪声向量。

上面所建立的观测方程为非线性方程，需要对其进行线性化，并离散化：

z(k)＝Hx(k)+v(k)

其中，H为线性化和离散化后的观测矩阵，为2×4维矩阵。

步骤2：根据步骤1中所建立的状态方程和观测方程，采用卡尔曼进行滤波，对第k+1时刻进行滤波；

$\hat{x}(k+1/k)=\mathrm{\Φ}(k+1,k)\hat{x}(k/k)$

$P(k+1/k)=\mathrm{\Φ}(k+1,k)P(k/k){\mathrm{\Φ}}^T(k+1,k)+{\hat{Q}}_k$

K(k+1)＝P(k+1/k)H^T(k+1)[H(k+1)P(k+1/k)H^T(k+1)+R_k+1]^-1

$\hat{x}(k+1/k+1)=\hat{x}(k+1/k)+K(k+1)[z(k+1)-H(k+1)\hat{x}(k+1/k)]$

P(k+1/k+1)＝P(k+1/k)-P(k+1/k)H^T(k+1)

·[H(k+1)P(k+1/k)H^T(k+1)+R_k+1]^-1H(k+1)P(k+1/k)

步骤3：根据系统的状态量、观测量以及观测方程求取当前信息及其滤波残差值(第k+1时刻)：

${\mathrm{\ϵ}}_{k+1}=z_{k+1}-H{\hat{x}}_{k+1/k+1}$

其中，ε_k+1是滤波残差，为2维向量，z_k+1为第k+1时刻的观测值，H为系统观测矩阵，

为第k+1时刻状态量的估计值。

步骤4：根据步骤3中所获得的滤波残差值计算状态噪声协方差阵的修正加权系数。假设系统的状态噪声和观测噪声均已经为白化后的白噪声，即状态噪声和观测噪声协方差阵均为对角线矩阵，将滤波残差值与观测噪声协方差阵中对应对角线上的值相比：

$g_1=\frac{{\mathrm{\ϵ}}_{1(k+1)}}{R_1}$ $g_2=\frac{{\mathrm{\ϵ}}_{2(k+1)}}{R_2}$

式中，i＝1～2，R₁，R₂为系统观测噪声协方差阵R对角线上的两个分量，设g＝[g₁ g₂]^T。

系统的观测量为2维，则构造如下的向量：

r＝[R₁ R₂]^T，

则计算的修正加权系数为：

k＝H′·(g+r)

计算出的修正系数k即为系统噪声协方差阵的修正系数向量，其为4维向量。

步骤5：修正系统噪协方差阵。

${\hat{q}}_j=k_j\·q_j$

${\hat{Q}}_{k+1}=\mathrm{diag}\left[\begin{array}{cccc}{\hat{q}}_1& {\hat{q}}_2& {\hat{q}}_3& {\hat{q}}_4\end{array}\right]$

其中，j＝1～4。

将本发明方法与背景技术中所提出的简化Sage-Husa自适应卡尔曼滤波方法进行对比，对于所建立的不确切状态模型，

$\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·\·}{x}\\ \stackrel{\·}{y}\\ \stackrel{\·\·}{y}\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 0& 0& 0\end{array}\right]\·\left[\begin{array}{c}x\\ \stackrel{\·}{x}\\ y\\ \stackrel{\·}{y}\end{array}\right]+\left[\begin{array}{c}0\\ w_1\\ 0\\ w_2\end{array}\right]$

从图3可以看出，采用简化Sage-Husa自适应卡尔曼滤波不能准确估计出目标的运动轨迹；从图4可以看出，采用本发明方法，对系统噪声协方差阵进行实时修正后，可以估计出系统状态变量。表1给出了两种滤波方法的估计结果，可以看出在系统状态模型不确切情况下，采用本发明介绍的自适应滤波算法的精度要优于简化的Sage-Husa方法。

表1不同方法的滤波误差均值对比

位置误差均值(m)	x轴	y轴
			背景技术方法	0.1231	0.3895
本发明方法	0.0056	0.0039

Claims (1)

1.一种系统模型未知的自适应卡尔曼滤波方法，其特征在于包括下述步骤：

(a)建立系统离散化和线性化后的状态方程：

x(k+1)＝Φx(k)+w(k)

式中，x为m维状态向量，Φ为m×m维状态转移矩阵，w为m维系统状态噪声，建立系统离散化和线性化后的观测方程：

z(k)＝Hx(k)+v(k)

式中，z为n维观测向量；H是n×m维观测矩阵，v为n维系统状态噪声；

(b)采用卡尔曼滤波方法，对第k+1时刻进行滤波；

$\hat{x}(k+1/k)=\mathrm{\Φ}(k+1,k)\hat{x}(k/k)$

$P(k+1/k)=\mathrm{\Φ}(k+1,k)P(k/k){\mathrm{\Φ}}^T(k+1,k)+{\hat{Q}}_k$

K(k+1)＝P(k+1/k)H^T(k+1)[H(k+1)P(k+1/k)H^T(k+1)+R_k+1]^-1

$\hat{x}(k+1/k+1)=\hat{x}(k+1/k)+K(k+1)[z(k+1)-H(k+1)\hat{x}(k+1/k)]$

P(k+1/k+1)＝P(k+1/k)-P(k+1/k)H^T(k+1)

·[H(k+1)P(k+1/k)H^T(k+1)+R_k+1]^-1H(k+1)P(k+1/k)

(c)滤波残差是，

${\mathrm{\ϵ}}_{k+1}=z_{k+1}-H{\hat{x}}_{k+1/k+1}$

式中，ε_k+1是滤波残差，z_k+1是第k+1时刻的观测值，H是系统观测矩阵，

是第k+1时刻状态量的估计值；

(d)修正加权系数是，

$g_i=\frac{{\mathrm{\ϵ}}_{i(k+1)}}{R_i}$

r＝[R₁ R₂…R_n]^T

k＝H′·(g+r)

式中，i＝1～n，R_i为观测噪声协方差阵R对角线上第i个数，k为修正加权系数，为m维向量；

(e)修正系统噪声协方差阵，

${\hat{q}}_j=k_j\·q_j$

${\hat{Q}}_{k+1}=\mathrm{diag}\left[\begin{array}{cccc}{\hat{q}}_1& {\hat{q}}_2& ...& {\hat{q}}_m\end{array}\right]$

式中，j＝1～m，是估计出的第k+1时刻系统噪声协方差阵，

是修正后状态噪声协方差矩阵

中对角线上第j个分量，q_j是修正前系统噪声协方差阵Q_k+1对角线上第j个分量。

Images