CN104168005A - 带有未知观测噪声协方差阵递推估计的卡尔曼滤波方法 - Google Patents

Abstract

本发明提供了针对离散时间时不变***的一种带有未知观测噪声协方差阵递推估计的卡尔曼滤波方法，解决了离散时间线性时不变***中观测噪声协方差矩阵完全未知的情况下的***状态滤波估计问题。步骤一、利用观测序列构建新统计序列；步骤二、计算{ξ_k}的协方差矩阵递推公式；步骤三、计算观测噪声协方差矩阵估计序列{f(R)_k}；步骤四、计算出协方差矩阵的估计序列然后通过代数关系计算观测噪声协方差矩阵的实时估计；步骤五、将观测噪声的协方差矩阵估计序列替代真值代入标准卡尔曼滤波方法中，计算***实时的状态估计以及状态估计偏差的协方差矩阵。

Description

带有未知观测噪声协方差阵递推估计的卡尔曼滤波方法

技术领域

本发明属于离散时间自适应滤波领域，具体涉及一种带有未知观测噪声协方差阵递推估计的卡尔曼滤波方法。

背景技术

卡尔曼滤波理论由于其自身的优越性，经过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∈R^n×1为k时刻***状态，x_k-1为k-1时刻的***状态，A为状态转移矩阵，w_k-1为***过程噪声，C为观测矩阵，v_k为***观测噪声，y_k∈R^m×1为k时刻***观测；

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

M_o＝[C CA … CA^n-1]^T

其中[·]^T表示矩阵转置；

由于***可观测，则可观测矩阵M_o列满秩，即其存在左伪逆矩阵

$M_o^{+}={\left(M_o^T{M}_o\right)}^{-1}{M}_o^T$

并且左伪逆矩阵满足

$M_o^{+}{M}_o=I_{n\×n}$

其中I_n×n为n维单位矩阵；

其中Q已知，R未知；

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

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

${\mathrm{\ξ}}_k=y_k-C{A}^n{M}_o^{+}{\left[\begin{array}{cccc}{y}_{k-n}& y_{k-n}& ...& y_{k-1}\end{array}\right]}^T$

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

$\begin{array}{c}{\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\end{array}$

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

步骤三、利用观测噪声协方差矩阵与新统计序列协方差矩阵实时估计值Cov_k(ξ)之间的代数关系，计算观测噪声协方差矩阵估计序列{f(R)_k}:

f(R)_k＝Cov_k(ξ)-F(Q)

其中

$F\left(Q\right)=({\mathrm{CA}}^{n-1}+C_Q^{n-1})Q{({\mathrm{CA}}^{n-1}+C_Q^{n-1})}^T+...+(\mathrm{CA}+C_Q^1)Q{(\mathrm{CA}+C_Q^1)}^T+{\mathrm{CQC}}^T$

$C_Q^1={\mathrm{CA}}^n{M}_o^{+}{\left[\begin{array}{cccc}0& 0& ...& C\end{array}\right]}^T,C_Q^2={\mathrm{CA}}^n{M}_o^{+}{\left[\begin{array}{cccc}0& ...0& C& \mathrm{CA}\end{array}\right]}^T,...,C_Q^{n-1}={\mathrm{CA}}^n{M}_o^{+}{\left[\begin{array}{cccc}0& C& ...& {\mathrm{CA}}^{n-2}\end{array}\right]}^T$

步骤四、通过f(R)和观测噪声协方差矩阵R的关系，计算出协方差矩阵的估计序列从观测噪声序列与f(v)之间的关系

$f\left(v\right)=v_k-{\mathrm{CA}}^n{M}_o^{+}{\left[\begin{array}{cccc}{v}_{k-n}& v_{k-n+1}& ...& v_{k-1}\end{array}\right]}^T$

获取f(R)与观测噪声协方差矩阵的代数关系，然后通过代数关系计算观测噪声协方差矩阵的实时估计

步骤五、将观测噪声的协方差矩阵估计序列替代真值代入标准卡尔曼滤波方法中，计算***实时的状态估计以及状态估计偏差的协方差矩阵。

有益效果：

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

此外，从方法的实现可以看出基于递推协方差矩阵估计卡尔曼滤波方法形式简单，计算复杂度不高，易于计算机实现，利于工程实践***中应用和实现。

附图说明

图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∈R^n×1为k时刻***状态，x_k-1为k-1时刻的***状态，A为状态转移矩阵，w_k-1为***过程噪声，C为观测矩阵，v_k为***观测噪声，y_k∈R^m×1为k时刻***观测；

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

M_o＝[C CA … CA^n-1]^T

其中[·]^T表示矩阵转制；

由于***可观测，则可观测矩阵M_o列满秩，即其存在左伪逆矩阵

$M_o^{+}={\left(M_o^T{M}_o\right)}^{-1}{M}_o^T$

并且左伪逆矩阵满足

$M_o^{+}{M}_o=I_{n\×n}$

其中I_n×n为n维单位矩阵；

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

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

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

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

从式(1)可知，

＝CAx_k-1+Cw_k-1+v_k   (2)

$\left[\begin{array}{c}{y}_{k-n}\\ y_{k-n+1}\\ .\\ .\\ .\\ y_{k-1}\end{array}\right]=M_o{x}_{k-n}+\left[\begin{array}{c}{v}_{k-n}\\ v_{k-n+1}\\ .\\ .\\ .\\ v_{k-1}\end{array}\right]+\left[\begin{array}{c}0\\ C\\ .\\ .\\ .\\ {\mathrm{CA}}^{n-2}\end{array}\right]w_{k-n}+\left[\begin{array}{c}0\\ 0\\ C\\ .\\ .\\ .\\ {\mathrm{CA}}^{n-3}\end{array}\right]w_{k-n+1}+...+\left[\begin{array}{c}0\\ .\\ .\\ .\\ 0\\ C\end{array}\right]w_{k-2}---\left(3\right)$

由于可观测矩阵存在左伪逆矩阵，从式(3)中可知，

$x_{k-n}=M_o^{+}\left[\begin{array}{c}{y}_{k-n}\\ y_{k-n+1}\\ .\\ .\\ .\\ y_{k-1}\end{array}\right]-M_o^{+}\left[\begin{array}{c}{v}_{k-n}\\ v_{k-n+1}\\ .\\ .\\ .\\ v_{k-1}\end{array}\right]-M_o^{+}\left[\begin{array}{c}0\\ C\\ .\\ .\\ .\\ {\mathrm{CA}}^{n-2}\end{array}\right]w_{k-n}-M_o^{+}\left[\begin{array}{c}0\\ 0\\ C\\ .\\ .\\ .\\ {\mathrm{CA}}^{n-3}\end{array}\right]w_{k-n+1}-...-M_o^{+}\left[\begin{array}{c}0\\ .\\ .\\ .\\ 0\\ C\end{array}\right]w_{k-2}---\left(4\right)$

将式(4)代入式(2)整理可得，

$y_k-{\mathrm{CA}}^n{M}_o^{+}{\left[\begin{array}{cccc}{y}_{k-n}& y_{k-n+1}& ...& y_{k-1}\end{array}\right]}^T=v_k-{\mathrm{CA}}^n{M}_o^{+}{\left[\begin{array}{cccc}{v}_{k-n}& v_{k-n+1}& ...& v_{k-1}\end{array}\right]}^T+F^{\′}\left(Q\right)---\left(5\right)$

其中 $F^{\′}\left(w\right)=({\mathrm{CA}}^{n-1}+C_Q^{n-1})w_{k-n}+...+(\mathrm{CA}+C_Q^1)w_{k-2}+{\mathrm{Cw}}_{k-1}.$

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

${\mathrm{\ξ}}_k=y_k-{\mathrm{CA}}^n{M}_o^{+}{\left[\begin{array}{cccc}{y}_{k-n}& y_{k-n+1}& ...& y_{k-1}\end{array}\right]}^T---\left(6\right)$

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

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

依据大数定律可知，对于随机变量φ∈R^m×1，且E(φ)＝0，则随机变量φ的协方差矩阵可以通过下式求取：

$\mathrm{Cov}\left(\mathrm{\φ}\right)=E\left({\mathrm{\φ\φ}}^T\right)=\underset{n\→\∞}{\lim }{\mathrm{Cov}}_n\left(\mathrm{\φ}\right)=\underset{n\→\∞}{\lim }\frac{1}{n-1}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\φ}}_i{\mathrm{\φ}}_i^T---\left(7\right)$

其中E(·)表示随机变量的数学期望，Cov(·)表示随机变量的协方差矩阵，R^m×1表示m维实数空间。φ_i为随机变量φ的取值，Cov_n(·)则表示随机变量φ的取值序列{φ_n}的协方差矩阵。

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

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

$\begin{array}{c}{\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\end{array}---\left(8\right)$

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

步骤三、利用观测噪声协方差矩阵与新统计序列协方差矩阵实时估计值Cov_k(ξ)之间的代数关系，计算观测噪声协方差矩阵估计序列{f(R)_k}；

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

从式(5)可得

f(R)＝Cov(ξ)-F(Q)   (9)

由于***过程噪声的协方差矩阵Q精确已知，可得

$F\left(Q\right)=({\mathrm{CA}}^{n-1}+C_Q^{n-1})Q{({\mathrm{CA}}^{n-1}+C_Q^{n-1})}^T+...+(\mathrm{CA}+C_Q^1)Q{(\mathrm{CA}+C_Q^1)}^T+\mathrm{CQ}{C}^T$

由步骤二的处理过程可以获得随机变量ξ的协方差矩阵的实时估计值Cov_k(ξ)。从式(9)可得观测噪声构造新序列的协方差矩阵实时估计值f(R)_k：

f(R)_k＝Cov_k(ξ)-Cov(V)   (10)

通过式(10)可以从步骤二的随机变量ξ_k的协方差估计值Cov_k(ξ)中得到观测噪声重构序列的协方差矩阵的实时估计序列{f(R)_k}。

步骤四、通过f(R)和观测噪声协方差矩阵R的关系，计算出协方差矩阵的估计序列

从观测噪声序列与f(v)之间的关系

$f\left(v\right)=v_k-{\mathrm{CA}}^n{M}_o^{+}{\left[\begin{array}{cccc}{v}_{k-n}& v_{k-n+1}& ...& v_{k-1}\end{array}\right]}^T$

可以获取f(R)与观测噪声协方差矩阵的代数关系，然后通过代数关系计算观测噪声协方差矩阵的实时估计

${\hat{R}}_k=f^{-1}{\left(R\right)}_k$

步骤五、利用观测噪声的协方差矩阵估计序列作为参数代入标准卡尔曼滤波方法中，计算***实时的状态估计以及状态估计偏差的协方差矩阵。

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

时间更新：

$\left\{\begin{array}{c}{\hat{x}}_{k,k-1}=A{\hat{x}}_{k-1}\\ P_{k,k-1}=A{P}_{k-1}{A}^T+Q\end{array}\right.---\left(11\right)$

观测更新：

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

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

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

Claims (1)

1.一种带有未知观测噪声协方差阵递推估计的卡尔曼滤波方法，该方法所针对的离散时间线性时不变***模型为：

$\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∈R^n×1为k时刻***状态，x_k-1为k-1时刻的***状态，A为状态转移矩阵，w_k-1为***过程噪声，C为观测矩阵，v_k为***观测噪声，y_k∈R^m×1为k时刻***观测；

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

M_o＝[C CA … CA^n-1]^T

其中[·]^T表示矩阵转置；

由于***可观测，则可观测矩阵M_o列满秩，即其存在左伪逆矩阵

$M_o^{+}={\left(M_o^T{M}_o\right)}^{-1}{M}_o^T$

并且左伪逆矩阵满足

$M_o^{+}{M}_o=I_{n\×n}$

其中I_n×n为n维单位矩阵；

其中Q已知，R未知；

针对上述离散时间线性时不变***的模型，其特征在于，包括如下步骤：

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

${\mathrm{\ξ}}_k=y_k-C{A}^n{M}_o^{+}{\left[\begin{array}{cccc}{y}_{k-n}& y_{k-n}& ...& y_{k-1}\end{array}\right]}^T$

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

$\begin{array}{c}{\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\end{array}$

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

步骤三、利用观测噪声协方差矩阵与新统计序列协方差矩阵实时估计值Cov_k(ξ)之间的代数关系，计算观测噪声协方差矩阵估计序列{f(R)_k}:

f(R)_k＝Cov_k(ξ)-F(Q)

其中

$F\left(Q\right)=({\mathrm{CA}}^{n-1}+C_Q^{n-1})Q{({\mathrm{CA}}^{n-1}+C_Q^{n-1})}^T+...+(\mathrm{CA}+C_Q^1)Q{(\mathrm{CA}+C_Q^1)}^T+{\mathrm{CQC}}^T$

$C_Q^1={\mathrm{CA}}^n{M}_o^{+}{\left[\begin{array}{cccc}0& 0& ...& C\end{array}\right]}^T,C_Q^2={\mathrm{CA}}^n{M}_o^{+}{\left[\begin{array}{cccc}0& ...0& C& \mathrm{CA}\end{array}\right]}^T,...,C_Q^{n-1}={\mathrm{CA}}^n{M}_o^{+}{\left[\begin{array}{cccc}0& C& ...& {\mathrm{CA}}^{n-2}\end{array}\right]}^T$

步骤四、通过f(R)和观测噪声协方差矩阵R的关系，计算出协方差矩阵的估计序列从观测噪声序列与f(v)之间的关系

$f\left(v\right)=v_k-{\mathrm{CA}}^n{M}_o^{+}{\left[\begin{array}{cccc}{v}_{k-n}& v_{k-n+1}& ...& v_{k-1}\end{array}\right]}^T$

获取f(R)与观测噪声协方差矩阵的代数关系，然后通过代数关系计算观测噪声协方差矩阵的实时估计

步骤五、将观测噪声的协方差矩阵估计序列替代真值代入标准卡尔曼滤波方法中，计算***实时的状态估计以及状态估计偏差的协方差矩阵。