CN104868876A - 一种针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法 - Google Patents

Abstract

本发明提供一种针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法，具体过程为：步骤一，判断系统模型是否满足预设条件，若满足，则进入步骤二，否则结束该方法；步骤二，构建系统广义输出矩阵E；步骤三，利用所述矩阵E确定系统输出的耦合向量δ_i；步骤四、根据耦合向量δ_i构建系统输出的线性耦合；基于所述线性耦合，利用大数定律对过程噪声的协方差矩阵Q进行估计，将估计的结果记为步骤五、将过程噪声的协方差矩阵的实时估计结果带入到标准Kalman滤波器中，获得滤波结果。本发明方法可以实现过程噪声协方差矩阵Q未知情况下的Kalman滤波。

Description

一种针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法

技术领域

本发明涉及一种针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法，本发明属于Kalman滤波方法技术领域。

背景技术

Kalman滤波是一种时域滤波方法，采用状态空间方法对系统进行描述，算法采用递推形式，数据存储量小，在计算机上执行起来十分方便。其不仅可以处理平稳随机过程，还可以处理多维非平稳随机过程。当系统模型及噪声先验信息已知而且噪声满足高斯白噪声假设的前提下，可以证明Kalman滤波结果是最小方差无偏估计。因为Kalman滤波方法简单易行，结果准确可靠，具有其他滤波方法所不具备的优点，所以Kalman滤波作为一种重要的最优估计方法广泛的应用于各种领域，如惯性导航、制导系统、全球定位系统、通信与信号过程、随机最优控制以及故障诊断等应用领域。

Kalman滤波是建立在模型精确、随机干扰信号统计特性已知基础上的，但是对于一个实际应用系统，其中往往存在着模型不确定或者噪声信号统计特性不完全已知的情况，尤其是很难提前获得准确的过程噪声信号统计特性，这是因为在实际应用的过程中，受到环境变化、器件老化和系统冲击等因素的影响，系统过程噪声的统计特性会发生较大变化，同时在系统实际运行过程中很难对噪声的统计特性进行实时测量。对于过程噪声而言，其统计特性直接由其协方差矩阵Q决定；过程噪声的协方差矩阵Q中的对角线元素代表了过程噪声的方差，非对角线元素代表了不同过程噪声之间的协方差。统计特性的不确定性将直接导致滤波算法中所使用的Q和真实的Q之间存在较大的偏差，而该偏差会使得Kalman滤波算法失去最优性，估计精度大大下降，严重时甚至会引起滤波发散。所以如何使得Kalman滤波算法在噪声信号统计特性不完全已知的情况下仍然能获得最优的滤波结果具有十分重要的意义。

发明内容

有鉴于此，本发明的目的在于克服已有技术存在的不足，提出一种针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法。

本发明的目的是通过下述技术方案实现

一种针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法，具体过程为：

步骤一，判断系统模型是否满足预设条件，若满足，则进入步骤二，否则结束该方法；

所述预设条件为： $\mathrm{rank}\left[\begin{array}{cc}{H}^T& 0\\ 0& H^T\end{array}\right]=\mathrm{rank}\left[\begin{array}{ccc}{H}^T& 0& A^T{\mathrm{\η}}_i\\ 0& H^T& {\mathrm{\η}}_i\end{array}\right]$

其中，rank[]代表矩阵的秩，A为系统状态转移矩阵，H为系统观测矩阵，m为系统状态变量的个数；

步骤二，构建系统广义输出矩阵E；

步骤三，利用所述矩阵E确定系统输出的耦合向量δ_i；

步骤四、根据耦合向量δ_i构建系统输出的线性耦合；基于所述线性耦合，利用大数定律对过程噪声的协方差矩阵Q进行估计，将估计的结果记为

步骤五、将过程噪声的协方差矩阵的实时估计结果带入到标准Kalman滤波器中，获得滤波结果。

进一步地，本发明所述矩阵E定义为：

$E=\left[\begin{array}{cc}H& 0\\ \mathrm{HA}& H\end{array}\right].$

进一步地，本发明步骤四的具体过程为：

定义系统当前所处的时刻为l，同时定义其中代表向下取整；根据耦合向量δ_i计算输出的线性耦合

${\tilde{Z}}_1^i={\mathrm{\δ}}_i\left[\begin{array}{c}{Z}_1\\ Z_2\end{array}\right],{\tilde{Z}}_2^i={\mathrm{\δ}}_i\left[\begin{array}{c}{Z}_3\\ Z_4\end{array}\right],\·\·\·\·\·\·,{\tilde{Z}}_p^i={\mathrm{\δ}}_i\left[\begin{array}{c}{Z}_{2p-1}\\ Z_{2p}\end{array}\right]---\left(7\right)$

其中，Z₁,Z₂…Z_2p分别对应时刻1,2,…,2p的系统输出；

利用大数定律对过程噪声的协方差矩阵Q进行估计：

$Q_i=\frac{1}{p}[{\left({\tilde{Z}}_1^i\right)}^2+{\left({\tilde{Z}}_2^i\right)}^2+\·\·\·+{\left({\tilde{Z}}_p^i\right)}^2]-{\mathrm{\δ}}_i\left[\begin{array}{cc}R& 0\\ 0& R\end{array}\right]{\mathrm{\δ}}_i^T$

其中，R为系统量测噪声的协方差矩阵；

本发明的有益效果:

本发明与标准Kalman滤波方法相比，该方法可以应用于过程噪声的协方差矩阵Q未知的情况下，扩展了标准Kalman滤波的使用范围；同时因为该方法并没有对Kalman滤波方法的滤波计算回路做出改动，所以该滤波方法的滤波结果仍然是最优的。

附图说明

图1为本发明滤波方法的流程图；

图2为本发明的具体实施例中的过程噪声w1_k的方差估计值示意图；

图3为本发明的具体实施例中的过程噪声w2_k的方差估计值示意图；

图4为本发明的具体实施例中的状态1的滤波估计误差示意图；

图5为本发明的具体实施例中的状态2的滤波估计误差示意图；

图6为本发明的具体实施例中的状态3的滤波估计误差示意图。

具体实施方式

下面结合附图和具体实例对本发明进行详细说明。

本发明的基本原理是：在系统模型满足一定条件的基础上，将系统量测噪声的协方差矩阵R作为已知条件，将不同时刻的系统输出进行线性耦合用于消去系统状态变量，同时利用大数定律对该耦合输出的协方差矩阵进行估计，进而准确地估计过程噪声的协方差矩阵Q，最后将Q的估计结果实时提供给Kalman滤波器完成滤波计算，保证了即使在Q未知的情况下，滤波结果也仍然能具有足够的准确性。

一种针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法，如图1所示，具体过程为：

步骤一、判断系统模型是否满足使用本发明方法的预设条件；

考虑系统离散化后的模型如式1所示：

$\left\{\begin{array}{c}{X}_{k+1}={\mathrm{AX}}_k+{\mathrm{Bw}}_k\\ Z_k={\mathrm{HX}}_k+v_k\end{array}\right.---\left(1\right)$

其中，A为系统状态转移矩阵；H为系统观测矩阵；X_k+1和X_k分别代表第k+1步和第k步的系统状态；Z_k代表系统第k步的观测值，下标k和k+1代表迭代步数，均为正整数；Bw_k表示第k步的系统过程噪声；v_k表示第k步的系统量测噪声；系统的过程噪声和量测噪声均为互不相关的高斯白噪声。

系统状态变量的个数为m，系统观测值的个数为n；则Bw_k和v_k可以进一步定义为如下形式：

${\mathrm{Bw}}_k=\left[\begin{array}{c}{w}_{1k}\\ w_{2k}\\ .\\ .\\ .\\ w_{\mathrm{mk}}\end{array}\right]---\left(2\right)$

$v_k=\left[\begin{array}{c}{v}_{1k}\\ v_{2k}\\ .\\ .\\ .\\ v_{\mathrm{nk}}\end{array}\right]---\left(3\right)$

对于过程噪声Bw_k，其协方差矩阵为Q，其维数为m×m；对于量测噪声v_k，其协方差矩阵为R，其维数为n×n；Q和R均为对角矩阵，其非对角线元素均为0，Q中的对角线元素依序为Q₁,…,Q_m。

基于此模型，给出该滤波方法的使用条件：

该方法的核心在于确定Q，即确定Q₁,…,Q_m。此处以求取为例进行说明。

定义向量则该滤波方法的使用条件可以表达为如下形式：

对于任意满足的i，均有下式成立：

$\mathrm{rank}\left[\begin{array}{cc}{H}^T& 0\\ 0& H^T\end{array}\right]=\mathrm{rank}\left[\begin{array}{ccc}{H}^T& 0& A^T{\mathrm{\η}}_i\\ 0& H^T& {\mathrm{\η}}_i\end{array}\right]---\left(4\right)$

其中，rank[]代表矩阵的秩，系统模型满足式(4)中的公式同时R已知，为该方法的使用条件。

步骤二、在满足本发明使用条件的基础上，构建系统广义输出矩阵E。

利用系统模型，可知如下矩阵等式成立：

$\left[\begin{array}{c}{Z}_1\\ Z_2\end{array}\right]=\left[\begin{array}{cc}H& 0\\ \mathrm{HA}& H\end{array}\right]\left[\begin{array}{c}{X}_1\\ w_1\end{array}\right]+\left[\begin{array}{c}{v}_1\\ v_2\end{array}\right]---\left(5\right)$

本实施例中较佳的将矩阵 $\left[\begin{array}{cc}H& 0\\ \mathrm{HA}& H\end{array}\right]$ 定义为系统的广义输出矩阵，用E来代表。

步骤三、利用所述矩阵E确定耦合向量δ_i。

耦合向量δ_i需要使得如下方程成立：

由于该系统模型满足该方法的使用条件，所以该方程一定有解，即一定可以确定出δ_i满足式(6)。

步骤四、根据耦合向量δ_i构建系统输出的线性耦合，进而对过程噪声的协方差矩阵Q进行估计。

定义系统当前所处的时刻为l，同时定义其中代表向下取整；根据耦合向量δ_i计算输出的线性耦合

${\tilde{Z}}_1^i={\mathrm{\δ}}_i\left[\begin{array}{c}{Z}_1\\ Z_2\end{array}\right],{\tilde{Z}}_2^i={\mathrm{\δ}}_i\left[\begin{array}{c}{Z}_3\\ Z_4\end{array}\right],\·\·\·\·\·\·,{\tilde{Z}}_p^i={\mathrm{\δ}}_i\left[\begin{array}{c}{Z}_{2p-1}\\ Z_{2p}\end{array}\right]---\left(7\right)$

其中，Z₁,Z₂…Z_2p分别对应时刻1,2,…,2p的系统输出。

利用式(7)中的可以进一步计算得到由大数定律可知，下述公式成立：

$\frac{1}{p}[{\left({\tilde{Z}}_1^i\right)}^2+{\left({\tilde{Z}}_2^i\right)}^2+\·\·\·+{\left({\tilde{Z}}_p^i\right)}^2]={\mathrm{\δ}}_i\left[\begin{array}{cc}R& 0\\ 0& R\end{array}\right]{\mathrm{\δ}}_i^T+Q_i---\left(8\right)$

式(8)可以化简为如下形式：

$Q_i=\frac{1}{p}[{\left({\tilde{Z}}_1^i\right)}^2+{\left({\tilde{Z}}_2^i\right)}^2+\·\·\·+{\left({\tilde{Z}}_p^i\right)}^2]-{\mathrm{\δ}}_i\left[\begin{array}{cc}R& 0\\ 0& R\end{array}\right]{\mathrm{\δ}}_i^T---\left(9\right)$

由于δ_i及R均为已知，则可以对Q_i进行实时估计。

令i依次等于1,…,m，则可以获取Q₁,Q₂,…,Q_m，进一步可以对Q进行估计，估计结果为

步骤五、将过程噪声协方差矩阵的实时估计结果带入到标准Kalman滤波器中，获得滤波结果。

定义通过上述方法实时估计得到的过程噪声协方差矩阵为令其替代标准Kalman滤波过程中的Q：

${\hat{X}}_{k,k-1}=A{\hat{X}}_{k-1}---\left(11\right)$

$P_{k,k-1}={\mathrm{AP}}_{k-1}{A}^T+\tilde{Q}---\left(12\right)$

K_k＝P_k,k-1H^T[HP_k,k-1H^T+R]^-1     (13)

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k[Z_k-H{\hat{X}}_{k,k-1}]---\left(14\right)$

P_k＝[I-K_kH]P_k,k-1     (15)

$\mathrm{\ΔX}=X_k-{\hat{X}}_k---\left(16\right)$

其中，代表上一步滤波结果，代表状态一步预测，P_k,k-1代表一步预测误差方差，K_k代表滤波增益，P_k代表当前步滤波误差方差，X_k代表状态真值，ΔX_k代表滤波误差。

通过上述步骤，便可以完成对系统状态的最优估计。

为了验证上述自适应滤波方法的准确性，本发明利用Matlab对该方法进行仿真验证，证明了该方法的有效性和准确性。

假设某离散系统模型如下所示：

$\left[\begin{array}{c}{x1}_{k+1}\\ {x2}_{k+1}\\ {x3}_{k+1}\end{array}\right]=\left[\begin{array}{ccc}1& T& 0.5T^2\\ 0& 1& T\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x1}_k\\ {x2}_k\\ {x3}_k\end{array}\right]+\left[\begin{array}{cc}1& 0\\ 0& 1\\ 0& 0\end{array}\right]\left[\begin{array}{c}{w1}_k\\ {w2}_k\end{array}\right]---\left(17\right)$

$Z_k=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right]\left[\begin{array}{c}{x1}_k\\ {x2}_k\\ {x3}_k\end{array}\right]+\left[\begin{array}{c}{v1}_k\\ {v2}_k\end{array}\right]---\left(18\right)$

其中T＝0.01，过程噪声的协方差矩阵为 $Q=\left[\begin{array}{cc}2& 0\\ 0& 3\end{array}\right],$ 量测噪声的协方差矩阵 $R=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right].$

在Kalman滤波过程中，除过程噪声协方差矩阵Q以外，系统模型的其他参数均为已知。

对该系统模型进行分析，可以发现该系统模型可以满足该滤波方法的使用条件，可以利用该方法对w1_k和w2_k的方差进行准确地实时估计。

w1_k的方差估计结果如附图2所示，w2_k的方差估计结果如附图3所示。采用该滤波方法对系统状态进行滤波，状态1的滤波误差如附图4所示,状态2的滤波误差如附图5所示,状态3的滤波误差如附图6所示。

从附图2和附图3中可以看出，该方法随着系统输出数目的增多，该方法估计得到w1_k的方差收敛于2,估计得到的w2_k的方差收敛于3,而且估计结果均很快的收敛到了相对应的真值附近,并且随着仿真步数的增加而逐渐趋向于真值,则可以说明该方法可以对w1_k的方差和w2_k的方差做出准确的估计。同时附图4、附图5和附图6说明，采用该方法对Q未知情况下的系统进行滤波，可以取得准确的滤波结果，并可以有效的限制滤波发散并提升滤波精度。

该结论证实了该滤波方法的准确性和其在实际工程上的使用价值。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进，或者对其中部分技术特征进行等同替换，这些改进和替换也应视为本发明的保护范围。

Claims (3)

1.一种针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法，其特征在，具体过程为：

步骤一，判断系统模型是否满足预设条件，若满足，则进入步骤二，否则结束该方法；

所述预设条件为： $\mathrm{rank}\left[\begin{array}{cc}{H}^T& 0\\ 0& H^T\end{array}\right]=\mathrm{rank}\left[\begin{array}{ccc}{H}^T& 0& A^T{\mathrm{\η}}_i\\ 0& H^T& {\mathrm{\η}}_i\end{array}\right]$

其中，rank[]代表矩阵的秩，A为系统状态转移矩阵，H为系统观测矩阵，m为系统状态变量的个数；

步骤二，构建系统广义输出矩阵E；

步骤三，利用所述矩阵E确定系统输出的耦合向量δ_i；

步骤四、根据耦合向量δ_i构建系统输出的线性耦合；基于所述线性耦合，利用大数定律对过程噪声的协方差矩阵Q进行估计，将估计的结果记为

步骤五、将过程噪声的协方差矩阵的实时估计结果带入到标准Kalman滤波器中，获得滤波结果。

2.根据权利1所述针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法，其特征在，所述矩阵E定义为：

$E=\left[\begin{array}{cc}H& 0\\ \mathrm{HA}& H\end{array}\right].$

3.根据权利1所述针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法，其特征在，所述步骤四的具体过程为：

定义系统当前所处的时刻为l，同时定义其中代表向下取整；根据耦合向量δ_i计算输出的线性耦合

${\tilde{Z}}_1^i={\mathrm{\δ}}_i\left[\begin{array}{c}{Z}_1\\ Z_2\end{array}\right],{\tilde{Z}}_2^i={\mathrm{\δ}}_i\left[\begin{array}{c}{Z}_3\\ Z_4\end{array}\right],......,{\tilde{Z}}_p^i={\mathrm{\δ}}_i\left[\begin{array}{c}{Z}_{2p-1}\\ Z_{2p}\end{array}\right]---\left(7\right)$

其中，Z₁,Z₂…Z_2p分别对应时刻1,2,…,2p的系统输出；

利用大数定律对过程噪声的协方差矩阵Q进行估计：

$Q_i=\frac{1}{p}[{\left({\tilde{Z}}_1^i\right)}^2+{\left({\tilde{Z}}_2^i\right)}^2+...+{\left({\tilde{Z}}_p^i\right)}^2]-{\mathrm{\δ}}_i\left[\begin{array}{cc}R& 0\\ 0& R\end{array}\right]{\mathrm{\δ}}_i^T$

其中，R为系统量测噪声的协方差矩阵；