CN103684349A - 一种基于递推协方差阵估计的卡尔曼滤波方法 - Google Patents

Abstract

本发明公开了一种基于递推协方差阵估计的卡尔曼滤波方法，属于自适应滤波领域。该方法主要针对离散时间线性时不变系统模型，在系统噪声协方差矩阵完全未知时，能够从系统的观测序列中构建新的统计序列，利用基于大数定律设计的递推计算协方差矩阵估计方法实时计算新构建序列的协方差矩阵估计序列，通过构建序列的协方差矩阵与过程噪声的协方差矩阵的关系计算过程噪声协方差矩阵的估计序列，然后将过程噪声的协方差矩阵的实时估计值代替真实过程噪声协方差矩阵代入标准卡尔曼滤波方法递推计算系统状态的实时估计和估计偏差的协方差矩阵。本发明适用于标准的卡尔曼滤波。

Description

一种基于递推协方差阵估计的卡尔曼滤波方法

技术领域

本发明属于自适应滤波领域，具体涉及一种基于递推协方差估计的改进卡尔曼滤波方法。 

背景技术

卡尔曼滤波理论自1960年提出后，经过50多年的发展，如今卡尔曼滤波理论已经在不同的工程领域得到了理论推广与应用。 

卡尔曼滤波是一种时域滤波方法，采用状态空间方法描述系统，即从与被提取信号有关的量测量中，通过方法估计出所需信号。其中被估计信号是由白噪声激励引起的随机响应，激励源与响应之间的传递结构为系统方程，量测量与被估计量之间的函数关系为量测方程。在系统方程和量测方程已知的情况下，对信号进行估计，估计过程利用了如下信息：系统方程、量测方程、白噪声激励的统计特性、量测误差的统计特性。 

假设线性系统的系统参数和噪声的统计特性符合要求时，标准的卡尔曼滤波方法在最小方差和最大似然意义下是一种最优状态估计方法。标准的卡尔曼滤波方法通常是针对线性时不变系统，并且要求其系统噪声和观测噪声是零均值的高斯白噪声。 

在标准卡尔曼滤波方法中，过程噪声的协方差矩阵是不可或缺的重要参数变量。过程噪声的协方差矩阵表征系统模型中的系统状态不确定性动态误差的统计特性。在工程实践中，在许多情况下系统噪声和观测噪声的协方差矩阵常难以事先精确获知，当无法获取过程噪声协方差矩阵精确值时设计者常采用过程噪声的协方差矩阵的上限替代精确地协方差矩阵。这会破坏标准卡尔曼滤波方法的最优性，且如果替代值与真实协方差矩阵误差较大时，可能会引起标准卡尔曼滤波方法的性能大幅衰减甚至不能正常工作。 

一般的自适应卡尔曼滤波方法在线辨识过程噪声的协方差矩阵方法与系统状态实时估计相互耦合，这会增加闭环稳定性分析困难程度。 

如何使用标准的卡尔曼滤波的方法，在离散时间线性时不变系统中系统噪声协方差矩阵完全未知的情况下，对系统状态进行滤波估计是亟待解决的问题。 

发明内容

有鉴于此，本发明提供了一种基于递推协方差矩阵估计方法的改进卡尔曼滤波方法，目的是要解决离散时间线性时不变系统中系统噪声协方差矩阵完全未知的情况下的系统状态滤波估计问题。 

为达到上述目的，本发明的技术方案为：一种基于递推协方差阵估计的卡尔曼滤波方法，该方法所针对的离散时间线性时不变系统的模型为： 

$\left\{\begin{array}{c}{x}_k={\mathrm{Ax}}_{k-1}+w_{k-1}\\ y_k={\mathrm{Cx}}_k+v_k\end{array}\right.$

其中x_k为k时刻系统状态，x_k-1为k-1时刻的系统状态，A为状态转移矩阵，w_k-1为系统过程噪声，C为观测矩阵，v_k为系统观测噪声，y_k为k时刻系统观测，y_k-1为k-1时刻系统观测； 

其中A、C为常值矩阵且已知；观测矩阵C存在左伪逆矩阵M；其中由系统观测y_k组成的观测序列{y_k}有界；系统的过程噪声和观测噪声为不相关零均值高斯白噪声，其中观测噪声协方差矩阵为常值R、过程噪声协方差矩阵为常值矩阵Q； 

其中R已知，Q未知； 

针对上述离散时间线性时不变系统的模型，本方法包括如下步骤： 

步骤一、利用观测序列{y_k}构建新统计序列{ξ_k}： 

ξ_k＝My_k-AMy_k-1

步骤二、计算{ξ_k}的协方差矩阵递推公式： 

${\mathrm{Cov}}_k\left(\mathrm{\ξ}\right)=\frac{1}{k-1}\underset{i=1}{\overset{k}{\mathrm{\Σ}}}{\mathrm{\ξ}}_i{\mathrm{\ξ}}_i^T$

$=\frac{1}{k-1}[\underset{i=1}{\overset{k-1}{\mathrm{\Σ}}}{\mathrm{\ξ}}_i{\mathrm{\ξ}}_i^T+{\mathrm{\ξ}}_k{\mathrm{\ξ}}_k^T]$

$=\frac{k-2}{k-1}{\mathrm{Cow}}_{k-1}\left(\mathrm{\ξ}\right)+\frac{1}{k}{\mathrm{\ξ}}_k{\mathrm{\ξ}}_k^T$

使用上述的协方差矩阵递推公式计算新统计序列{ξ_k}的协方差矩阵实时估计值Cov_k(ξ)，Cov(·)为·的协方差矩阵； 

步骤三、利用过程噪声协方差矩阵实时估计值Cov_k(ξ)和新统计序列协方差矩阵之间的代数关系，计算过程噪声协方差矩阵估计序列

: 

${\hat{Q}}_k={\mathrm{Cov}}_k\left(\mathrm{\ξ}\right)-\mathrm{Cov}\left(V\right)$

其中Cov(V)＝MRM^T+AMRM^TA^T; 

步骤四、将过程噪声的协方差矩阵估计序列

替代真值代入标准卡尔曼滤波方法中，计算系统实时的状态估计以及状态估计偏差的协方差矩阵。 

有益效果： 

本发明相对于标准的卡尔曼滤波方法，减弱了对过程噪声协方差矩阵参数的要求，可以用来处理过程噪声协方差矩阵事先完全未知，但协方差矩阵为定常值情况下的系统状态滤波估计问题。从本发明中的基本递推协方差矩阵估计方法可知，由于过程噪声的协方差矩阵的计算与系统状态估计值无关，由大数定律可保证过程噪声的协方差矩阵的估计序列以概率1收敛于过程噪声协方差矩阵真值。在过程噪声协方差矩阵估计序列收敛于真值的前提下，结合卡尔曼滤波方法的黎卡提递推分析可以给出基于递推协方差矩阵估计卡尔曼滤波方法的闭环稳定性结果以保证满足假设的离散时间线性时不变系统模型中，系统状态估计序列和估计偏差协方差矩阵序列收敛于具有精确过程噪声协方差矩阵的标准卡尔曼滤波的状态估计序列和估计偏差协方差矩阵序列。 

此外，从方法的实现可以看出基于递推协方差矩阵估计卡尔曼滤波方法形式简单，有良好的实时性，利于工程实践系统中应用和实现。 

具体实施方式

本方法的基本原理为： 

本发明针对离散时间线性时不变系统模型，其系统噪声协方差矩阵完全未知时，能够从系统的观测序列中构建新的统计序列，利用基于大数定律设计的递推计算协方差矩阵估计方法实时计算新构建序列的协方差矩阵估计序列，通过构建序列的协方差矩阵与过程噪声的协方差矩阵的关系计算过程噪声协方差矩阵的估计序列，然后将过程噪声的协方差矩阵的实时估计值代替真实过程噪声协方差矩阵代入标准卡尔曼滤波方法递推计算系统状态的实时估计和估计偏差的协方差矩阵。 

下面结合实施例，对本发明进行详细描述。 

本实施例中，为了便于描述基于递推协方差估计卡尔曼滤波方法，我们首先给出离散时间线性时不变系统模型以及前提假设。 

本方法针对的可控、可观测的离散时间线性时不变系统的模型为： 

$\left\{\begin{array}{c}{x}_k={\mathrm{Ax}}_{k-1}+w_{k-1}\\ y_k={\mathrm{Cx}}_k+v_k\end{array}\right.---\left(1\right)$

其中x_k为k时刻系统状态，x_k-1为k-1时刻的系统状态，A为状态转移矩阵，w_k-1为系统过程噪声，C为观测矩阵，v_k为系统观测噪声，y_k为k时刻系统观测，y_k-1为k-1时刻系统观测。 

其中A、C为常值矩阵且精确已知；观测矩阵C存在左伪逆矩阵，左伪逆矩阵为[C^TC]^-1C^T。 

由于本发明是基于大数定律从观测序列中重构序列给出过程噪声协方差矩阵的估计序列，所以要求观测序列｛y_k｝有界以满足滤波方法的收敛条件。 

上述系统的过程噪声和观测噪声为不相关零均值高斯白噪声，其中观测噪声协方差矩阵为常值R、精确已知，过程噪声协方差矩阵为常值矩阵Q、事先完全未知。 

针对上述过程噪声协方差矩阵完全未知的系统，本发明的具体实施步骤如 下： 

步骤一、利用观测序列｛y_k｝构建新统计序列｛ξ_k｝。 

从式（1）可知， 

y_k＝Cx_k+v_k

＝C[Ax_k-1+w_k-1]+v_k（2） 

＝CAx_k-1+Cw_k-1+v_k

y_k-1＝Cx_k-1+v_k-1（3） 

从式（3）中可知， 

Cx_k-1＝y_k-1-v_k-1（4） 

由于观测矩阵C存在左伪逆矩阵，即 

[C^TC]^-1C^TC＝I（5） 

其中I为单位矩阵。 

式（4）两边同乘C的左伪逆矩阵可得 

x_k-1＝[C^TC]^-1C^T _yk-1-[C^TC]^-1C^Tv_k-1（6） 

将式（6）代入式（2）整理可得， 

My_k-AMy_k-1＝w_k-1+Mv_k-AMv_k-1（7） 

其中M＝[C^TC]^-1C^T。 

利用观测序列重构新统计序列｛ξ_k｝的定义如下： 

ξ_k＝My_k-AMy_k-1（8） 

上述所重构的新统计序列｛ξ_k｝，其期望E(ξ_k)=0，而且｛ξ_k｝与系统的状态估计无关，即｛ξ_k｝不与系统状态估计耦合，从而能够方便后续的数据处理。 

步骤二、依据大数定律，设计递推协方差矩阵估计方法计算｛ξ_k｝协方差矩阵估计序列： 

依据大数定律可知，对于随机变量

∈R^m×1，且E

=0，则随机变量

的协方差矩阵可以通过下式求取： 

其中E(·)表示随机变量的数学期望，Cov(·)表示随机变量的协方差矩阵， R^m×1表示m维实数空间。

为随机变量

的取值，Cov_n(·)则表示随机变量

的取值序列

的协方差矩阵。 

但是这种协方差矩阵计算方法适用于获取全部采样数据后数据处理过程，不能满足在线实时计算随机变量ξ的协方差矩阵估计序列，为了满足实时性要求需要对式（9）改进成为递推计算形式。 

零均值新统计序列｛ξ_k｝的协方差矩阵Cov_k(ξ)的递推公式为： 

${\mathrm{Cov}}_k\left(\mathrm{\ξ}\right)=\frac{1}{k-1}\underset{i=1}{\overset{k}{\mathrm{\Σ}}}{\mathrm{\ξ}}_i{\mathrm{\ξ}}_i^T$

$=\frac{1}{k-1}[\underset{i=1}{\overset{k-1}{\mathrm{\Σ}}}{\mathrm{\ξ}}_i{\mathrm{\ξ}}_i^T+{\mathrm{\ξ}}_k{\mathrm{\ξ}}_k^T]---\left(10\right)$

$=\frac{k-2}{k-1}{\mathrm{Cov}}_{k-1}\left(\mathrm{\ξ}\right)+\frac{1}{k}{\mathrm{\ξ}}_k{\mathrm{\ξ}}_k^T$

式（10）是一种递推递推求解随机变量ξ的协方差矩阵实时估计值的公式，通过式（8）和式（10）可以得到随机变量协方差矩阵实时估计值Cov_k(ξ)。 

步骤三、利用过程噪声协方差矩阵和新统计序列协方差矩阵之间的代数关系，计算过程噪声协方差矩阵估计值序列

。 

本发明的核心部分是在过程噪声协方差矩阵事先完全未知情况下处理系统滤波估计问题。在上两步中利用观测序列和系统参数构建了一个新的统计序列，并且通过基本的递推协方差矩阵估计方法得到序列的协方差矩阵估计序列，本步骤是在前两步的基础上给出过程噪声协方差矩阵实时估计方法。 

从式（7）可得 

Cov_k(ξ)＝Q+Cov(Mv_k-AMv_k-1)（11） 

由于观测噪声的协方差矩阵R精确已知，令V=Mv_k-AMv_k-1可得 

Cov(V)＝Cov(Mv_k-AMv_k-1)＝MRM^T+AMRM^TA^T（12） 

由步骤二的处理过程可以获得随机变量ξ的协方差矩阵的实时估计值Cov_k(ξ)。从式（11）可得过程噪声协方差矩阵实时估计值

： 

$\widehat{Q_k}={\mathrm{Cov}}_k\left(\mathrm{\ξ}\right)-\mathrm{Cov}\left(V\right)---\left(13\right)$

通过式（13）可以从步骤二的随机变量ξ_k的协方差估计值Cov_k(ξ)中得到过 程噪声的协方差矩阵的实时估计序列

。 

步骤四、利用过程噪声的协方差矩阵估计序列

作为参数代入标准卡尔曼滤波方法中，计算系统实时的状态估计以及状态估计偏差的协方差矩阵。 

过程噪声协方差矩阵是标准卡尔曼滤波方法的一项重要参数，若无法获得精确地过程噪声协方差矩阵则标准卡尔曼滤波方法不能正常工作。本发明所处理的问题是过程噪声协方差矩阵事先完全未知情况下的系统状态滤波估计问题，通过上述三个步骤可以获得实时的过程噪声协方差矩阵的估计序列，然后可将估计序列替代真值代入标准卡尔曼滤波方法中获得实时的状态估计及估计偏差的协方差矩阵。其处理过程与标准卡尔曼滤波方法类似，可以分为：时间更新和观测更新两部分。 

时间更新： 

$\left\{\begin{array}{c}{\hat{x}}_{k,k-1}={\widehat{\mathrm{Ax}}}_{k-1}\\ P_{k,k-1}={\mathrm{AP}}_{k-1}{A}^T+{\hat{Q}}_k\end{array}---\left(14\right)\right.$

观测更新： 

$\left\{\begin{array}{c}{K}_k=P_{k,k-1}{C}^T[{\mathrm{CP}}_{k,k-1}{C}^T+{]}^{-1}\\ {\hat{x}}_k={\hat{x}}_{k,k-1}+K_k[y_k-{\widehat{\mathrm{Cx}}}_{k,k-1}]\\ P_k=[I-K_{k}C]P_{k,k-1}\end{array}\right.---\left(15\right)$

其中为k时刻系统状态预估值，P_k,k-1为其协方差矩阵；

为k时刻状态估计值，P_k为k时刻状态估计偏差的协方差矩阵；K_k为卡尔曼滤波增益。 

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。 

Claims (1)

1.一种基于递推协方差阵估计的卡尔曼滤波方法，该方法所针对的离散时间线性时不变系统的模型为：

$\left\{\begin{array}{c}{x}_k=A{x}_{k-1}+w_{k-1}\\ y_k={\mathrm{Cx}}_k+v_k\end{array}\right.$

其中x_k为k时刻系统状态，x_k-1为k-1时刻的系统状态，A为状态转移矩阵，w_k-1为系统过程噪声，C为观测矩阵，v_k为系统观测噪声，y_k为k时刻系统观测，y_k-1为k-1时刻系统观测；

其中A、C为常值矩阵且已知；观测矩阵C存在左伪逆矩阵M；其中由系统观测y_k组成的观测序列{y_k}有界；系统的过程噪声和观测噪声为不相关零均值高斯白噪声，其中观测噪声协方差矩阵为常值R、过程噪声协方差矩阵为常值矩阵Q；

所述R已知，Q未知；

针对上述离散时间线性时不变系统的模型，本方法的特征在于，包括如下步骤：

步骤一、利用观测序列{y_k}构建新统计序列{ξ_k}：

ξ_k＝My_k-AMy_k-1

步骤二、计算{ξ_k}的协方差矩阵递推公式：

${\mathrm{Cov}}_k\left(\mathrm{\ξ}\right)=\frac{1}{k-1}\underset{i=1}{\overset{k}{\mathrm{\Σ}}}{\mathrm{\ξ}}_i{\mathrm{\ξ}}_i^T$

$=\frac{1}{k-1}[\underset{i=1}{\overset{k-1}{\mathrm{\Σ}}}{\mathrm{\ξ}}_i{\mathrm{\ξ}}_i^T+{\mathrm{\ξ}}_k{\mathrm{\ξ}}_k^T]$

$=\frac{k-2}{k-1}{\mathrm{Cov}}_{k-1}\left(\mathrm{\ξ}\right)+\frac{1}{k}{\mathrm{\ξ}}_k{\mathrm{\ξ}}_k^T$

使用上述的协方差矩阵递推公式计算新统计序列{ξ_k}的协方差矩阵实时估计值Cov_k(ξ)；Cov(·)为·的协方差矩阵；

步骤三、利用过程噪声协方差矩阵实时估计值Cov_k(ξ)和新统计序列协方差矩阵之间的代数关系，计算过程噪声协方差矩阵估计序列

${\hat{Q}}_k={\mathrm{Cov}}_k\left(\mathrm{\ξ}\right)-\mathrm{Cov}\left(V\right)$

其中Cov(V)＝MRM^T+AMRM^TA^T;

步骤四、将过程噪声的协方差矩阵估计序列

替代真值代入标准卡尔曼滤波方法中，计算系统实时的状态估计以及状态估计偏差的协方差矩阵。