CN104270119A - 基于非线性未知随机偏差的两阶段容积卡尔曼滤波方法 - Google Patents

Abstract

本发明涉及一种基于非线性未知随机偏差的二阶段容积卡尔曼滤波方法。本发明对于不含未知偏差的部分，算出一步预测值、一步预测方差、一步预测协方差。从而算出增益阵、估计值、估计误差方差。对于含未知偏差的部分，算出一步偏差估计、一步偏差估计方差、一步偏差估计协方差。再算出偏差增益阵、偏差对于状态变量的一步增益阵和估计增益阵。通过两个部分算出来的值，算出含未知偏差非线性系统的一步预测值，一步预测误差方差，估计值和估计误差方差。在完成滤波的同时，避免了对计算机来说大的计算量和维度灾难。

Description

基于非线性未知随机偏差的两阶段容积卡尔曼滤波方法

技术领域

本发明属于非线性系统的滤波领域，特别涉及一种处理含有未知随机变量非线性系统的两阶容积卡尔曼滤波方法。 

背景技术

非线性滤波是信号处理、目标跟踪和控制领域方面的热门话题之一，特别是，在卡尔曼滤波框架下的非线性滤波研究仍然是一个很火的问题，在近年来受到了越来越多的关注和研究。。 

卡尔曼滤波器“为线性高斯系统中最小均方差下的位置状态”提供了有效的解决方法。然而真实世界中，系统总是受到非线性和非高斯两方面的困扰。想要获得关于状态估计良好的办法是很难的。扩展卡尔曼于是受到了广泛的使用，无踪卡尔曼是另外一种用样本来解决非线性分布的方法。不幸的是上述提到的非线性滤波器都有发散和维度灾难的问题。一个更为精确的容积卡尔曼应运而生。容积卡尔曼滤波的核心是球型射线容积法则。它可以用来解决高斯非线性滤波问题，而不需要很大的计算量。 

标准的求容积卡尔曼滤波器要求精确的系统模型和精确的随机指标信息，在实际中模型通常包括未知定量和未知偏差。在处理含有位置偏差的动态系统的状态估计时，将偏差视为一部分系统状态，并且像系统一样估计偏差。这就引出了增广状态卡尔曼滤波，它的应用却也存在相对大的计算量的问题。 

发明内容

本发明涉及一种处理还有未知偏差非线性系统的两阶段容积卡尔曼滤波器。该方法将含有位置偏差的非线性系统看作不含位置偏差估计和含有位置偏差估计两部分。从而在获得良好非线性滤波的情况下，避免了增广卡尔曼滤波所带来的计算量大的问题。对于不含未知偏差的部分，算出一步预测值、一步预测方差、一步预测协方差。从而算出增益阵、估计值、估计误差方差。对于含未知偏差的部分，算出一步偏差估计、一步偏差估计方差、一步偏差估计协方差。再算出偏差增益阵、偏差对于状态变量的一步增益阵和估计增益阵。通过两个部分算出来的值，算出含未知偏差非线性系统的一步预测值，一步预测误差方差，估计值和估计误差方差。在完成滤波的同时，避免了对计算机来说大的计算量和维度灾难。本发明的具体步骤如下： 

步骤1.计算不含有未知偏差部分的无偏差估计一步预测一步预测误差方差一步预测误差协方差

步骤2.计算不含有未知偏差部分的无偏差估计增益阵估计值和估计误差方差阵P_k|k。 

步骤3.计算含有未知偏差部分的，未知偏差一步预测b_k|k-1偏差一步预测方差偏差预测误差协方差和以及

步骤4.计算含有未知偏差部分的，偏差增益阵和偏差估计误差方差阵 以及偏差一步增益U_k|k-1和偏差增益。 

步骤5.根据以上四步计算未知偏差非线性系统的一步预测方差和一步预测误差协方差和最终目标状态的最优线性估计及其误差协方差  $P_{k|k}^{x*}.$

本发明有益效果：使用两阶段容积卡尔曼方法不仅能有效处理还有未知偏差的非线性系统，还有效避免了增广状态卡尔曼在处理同类问题时所产生的维度灾难和计算机计算量过大的问题。 

附图说明

图1为两阶段容积卡尔曼滤波的流程图。 

具体实施方式

下面首先为跟踪目标的运动状态建立模型，其次给出两阶段容积强滤波器的滤波公式，将其分为无偏差估计和有偏差估计两个步骤，下面结合图1详细介绍本发明的实施过程。 

1系统建模 

1.1给出如下非线性系统动态模型 

x_k＝f_k-1(x_k-1)+w_k,k-1

z_k＝h_k(x_k)+v_k

其中k≥1是时刻指数，x_k∈R^n×1表示系统状态(R^n×1为n×1维列向量全集)，z_k∈R^n×1是测量值列向量，f_k-1(·)以及h_k(·)都是可微函数。初始状态x₀服从为均值，P₀为方差且独立于w_k和v_k随机变量。w_k∈R^n×1和v_k∈R^m×1都是均值为零的高斯白噪声，其中 $\begin{array}{ccc}E\left[w_k{w}_j^T\right]=Q_k{\mathrm{\δ}}_{\mathrm{ij}}& E\left[v_k{v}_j^T\right]=R_k{\mathrm{\δ}}_{\mathrm{ij}}& E\left[w_k{v}_j^T\right]=0\end{array},$ δ_ij是克罗内克脉冲函数。 

在容积卡尔曼中，n维随机变量x_k以为均值P_k为方差的随机变量，它可以被容积点近似为其中权重值w_i为  $w_i=\frac{1}{m}(i=\mathrm{1,2},...m;m=2n).$

当非线性系统信息不完全时，离散两阶容积卡尔曼滤波的分析方程如下： 

$\left\{\begin{array}{c}{\hat{x}}_{k|k-1}^{*}={\hat{x}}_{k|k-1}+U_{k|k-1}{b}_{k|k-1}\\ {\hat{x}}_{k|k}={\hat{x}}_{k|k}+U_{k|k}{b}_{k|k}\\ P_{k|k-1}^{x*}=P_{k|k-1}^x+U_{k|k-1}{P}_{k|k-1}^b{U}_{k|k-1}^T\\ P_{k|k}^{x*}=P_{k|k}^x+U_{k|k}{P}_{k|k}^b{U}_{k|k}^T\end{array}\right.$

其中和b_k为两阶容积卡尔曼的状态向量，修正无偏差估计滤波器和偏差部分分别如下：(下标k|k表示通过前k时刻的观测值所得到的状态估计值，同理k|k-1表示的是根据前k-1个时刻的观测值得到k时刻的状态估计值。) 

A.修正无偏差部分： 

步骤1.计算不含有未知偏差部分的无偏差估计一步预测一步预测误差方差一步预测误差协方差

1).计算容积点一步递归x_i,k|k-1(其中k-1表示k-1时刻) 

x_i,k|k-1＝f(x_i,k-1|k-1)   (1) 

2).根据m个容积点的采样来求得一步预测容积点近似估计值

${\hat{x}}_{k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i{x}_{i,k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{x}_{i,k|k-1}---\left(2\right)$

3).一步预测误差估计方差(其中代表高斯白噪w_k-1的方差) 

$\begin{array}{c}{P}_{k|k-1}^x=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i(x_{i,k|k-1}-{\hat{x}}_{k|k-1}){(x_{i,k|k-1}-{\hat{x}}_{k|k-1})}^T+Q_{k-1}^x\\ =\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}(x_{i,k|k-1}-{\hat{x}}_{k|k-1}){(x_{i,k|k-1}-{\hat{x}}_{k|k-1})}^T+Q_{k-1}^x\end{array}---\left(3\right)$

(其中右上角标x表明是不含偏差部分的方差计算，包含^代表估计值) 

4).容积点一步观测值递归 

z_i,k|k-1＝h(x_i,k|k-1)   (4) 

5).通过m个容积点近似一步预测观测估计值 

${\hat{z}}_{k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i{z}_{i,k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{z}_{i,k|k-1}---\left(5\right)$

6).得到新息z_k以后，根据上一步得到的一步预测观测值，求其误差 

${\mathrm{\ϵ}}_k^x=z_k-{\hat{z}}_{k|k-1}=z_k-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{z}_{i,k|k-1}---\left(6\right)$

7).观测值预测方差 

$\begin{array}{c}{P}_{\mathrm{zz},k|k-1}^x=E\left[{\mathrm{\ϵ}}_k^x{\mathrm{\ϵ}}_k^{\mathrm{xT}}\right]\\ =\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i(z_{i,k|k-1}-{\hat{z}}_{k|k-1}){(z_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T+R_k\\ =\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}(z_{i,k|k-1}-{\hat{z}}_{k|k-1}){(z_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T+R_k\end{array}---\left(7\right)$

8).一步预测状态与观测协方差 

$\begin{array}{c}{P}_{\mathrm{xz},k|k-1}^x=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i(x_{i,k|k-1}-{\hat{x}}_{k|k-1}){(z_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T\\ =\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}(x_{i,k|k-1}-{\hat{x}}_{k|k-1}){(z_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T\end{array}---\left(8\right)$

步骤2.计算不含有未知偏差部分的无偏差估计增益阵估计值和估计误差方差阵P_k|k

1).根据上两步得到一步预测增益值 

$\begin{array}{c}{K}_k^x=P_{k|k-1}^x{N}_k^{\mathrm{xT}}{P}_{\mathrm{zz},k|k-1}^{x-1}\\ =P_{\mathrm{xz},k|k-1}^x{P}_{\mathrm{zz},k|k-1}^{x-1}\end{array}---\left(9\right)$

其中由 $N_k^x={\left[P_{k|k-1}^{x-1}{P}_{k|k-1}^x\right]}^T$

2).根据上一步的预测增益值可以得到状态估计 

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k^x{\mathrm{\ϵ}}_k^x---\left(10\right)$

3).根据(3)、(7)、(10)状态估计误差方差 

$P_{k|k}^x=P_{k|k-1}^x-K_k^x{P}_{\mathrm{zz},k|k-1}^x{K}_k^{\mathrm{xT}}---\left(11\right)$

B.偏差估计部分 

两阶容积卡尔曼可以参考增广状态容积卡尔曼，通过扩展卡尔曼滤器所取 代，一些新的方程如下： 

步骤3.计算含有未知偏差部分的，未知偏差一步预测b_k|k-1偏差一步预测方差偏差预测误差协方差以及

1).一步预测观测值误差 

$\begin{array}{c}{\mathrm{\ϵ}}_k^b=z_k-{\hat{z}}_{k|k-1}-N_k^b{b}_{k|k-1}\\ ={\mathrm{\ϵ}}_k^x-N_k^b{b}_{k|k-1}\end{array}---\left(12\right)$

(其中右上角标b表示的是含偏差部分) 

其中是由之后的耦合方程求出的耦合系数。 

2).一步预测观测值误差协方差 

$\begin{array}{c}{P}_{\mathrm{zz},k|k-1}^b=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i(z_{i,k|k-1}-{\hat{z}}_{k|k-1}){(z_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T+R_k+N_k^b{P}_{k|k-1}^b{N}_k^{\mathrm{bT}}\\ =\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\left(z_{i,k|k-1}-{\hat{z}}_{k|k-1}\right){(z_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T+R_k+N_k^b{P}_{k|k-1}^b{N}_k^{\mathrm{bT}}\end{array}---\left(13\right)$

3).一步预测观测误差协方差 

$\begin{array}{c}{P}_{\mathrm{xz},k|k-1}^b=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i(x_{i,k|k-1}-{\hat{x}}_{k|k-1}){(z_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T+D_{k-1}{P}_{k|k-1}^b{N}_k^{\mathrm{bT}}\\ =\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\left(x_{i,k|k-1}-{\hat{x}}_{k|k-1}\right){(z_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T-D_{k-1}{P}_{k|k-1}^b{N}_k^{\mathrm{bT}}\end{array}---\left(14\right)$

$P_{k|k-1}^{\mathrm{xb}}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}(x_{i,k|k-1}-{\hat{x}}_{k|k-1}){(b_k-{\hat{b}}_{k|k-1})}^T---\left(15\right)$

为了补偿两阶段容积滤波器信息不完整所带来的影响，创新协方差如以上两个式子所示，接下来我们考虑两阶容积卡尔曼滤波器的偏差问题。 

4).状态偏差一步预测(由于噪声均值为零，等同于前一时刻的状态偏差) 

b_k|k-1＝b_k-1|k-1   (16) 

5).关于误差偏差b的一步偏差预测误差方差阵

$P_{k|k-1}^b=P_{k-1|k-1}^b+Q_{k-1}^b---\left(17\right)$

其中是上一步的估计误差方差，是一步状态偏差噪声的方差值 

6).状态偏差值迭代 

$\begin{array}{c}{b}_{k|k}=b_{k-1|K-1}+K_k^b({\mathrm{\ϵ}}_k^x-N_k^b{b}_{k-1|k-1})\\ =b_{k|k-1}+K_k^b{\mathrm{\ϵ}}_k^b\end{array}---\left(18\right)$

其中为耦合方程 $N_k^b={\left[P_{k|k-1}^{b-1}{P}_{\mathrm{xz},k|k-1}^b\right]}^T$ 所求的值 

7).通过(16)式算出的一步偏差预测方差阵求得

$P_{k|k}^b=(I-K_k^b{N}_k^b)P_{k|k-1}^b---\left(19\right)$

其中为耦合方程 $N_k^b={\left[P_{k|k-1}^{b-1}{P}_{\mathrm{xz},k|k-1}^b\right]}^T$ 所求的值 

步骤4.计算含有未知偏差部分的，偏差增益阵和偏差估计误差方差阵 以及偏差一步增益U_k|k-1和偏差增益 

1).计算关于状态误差增益系数阵

$\begin{array}{c}{K}_k^b=P_{k|k-1}^b{N}_k^{\mathrm{bT}}{[\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i(z_{i,k|k-1}-{\hat{z}}_{k|k-1}){(z_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T+R_k+N_k^b{P}_{k|k-1}^b{N}_k^{\mathrm{bT}}]}^{-1}\\ =P_{k|k-1}^b{N}_k^{\mathrm{bT}}{(P_{\mathrm{zz},k|k-1}^x+N_k^b{P}_{k|k-1}^b{N}_k^{\mathrm{bT}})}^{-1}\\ =P_{k|k-1}^b{N}_k^{\mathrm{bT}}{\left(P_{\mathrm{zz},k|k-1}^b\right)}^{-1}\end{array}---\left(20\right)$

2).耦合方程如下 

$N_k^x={\left[P_{k|k-1}^{x-1}{P}_{\mathrm{xz},k|k-1}^x\right]}^T---\left(21a\right)$

$N_k^b={\left[P_{k|k-1}^{b-1}{P}_{\mathrm{xz},k|k-1}^b\right]}^T---\left(21b\right)$

$U_{k|k-1}=P_{k|k-1}^{\mathrm{xb}}{P}_{\mathrm{xz},k|k-1}^{b-1}---\left(21c\right)$

$U_{k|k}=U_{k|k-1}-K_k^x{N}_k^b---\left(21d\right)$

步骤5.根据以上四步计算未知偏差非线性系统的一步预测方差和一步预测误差协方差和最终目标状态的最优线性估计及其误差协方差  $P_{k|k}^{x*};$

$\left\{\begin{array}{c}{\hat{x}}_{k|k-1}^{*}={\hat{x}}_{k|k-1}+U_{k|k-1}{b}_{k|k-1}\\ {\hat{x}}_{k|k}={\hat{x}}_{k|k}+U_{k|k}{b}_{k|k}\\ P_{k|k-1}^{x*}=P_{k|k-1}^x+U_{k|k-1}{P}_{k|k-1}^b{U}_{k|k-1}^T\\ P_{k|k}^{x*}=P_{k|k}^x+U_{k|k}{P}_{k|k}^b{U}_{k|k}^T\end{array}\right.$

将A.修正无偏差部分所算出来的和B.含偏差部分所求出来的U_k|k-1,b_k|k-1,U_k|k,b_k|k,带入到离散两阶容积卡尔曼滤波的分析方程中，便可以得到由上一步状态估计和状态估计偏差在得到信息之后得到下一步的状态估计和状态估计偏差。 

所以当我们知道关于偏差估计部分的初始条件： 

$\begin{array}{cc}{\hat{x}}_{0|0}=x_0-U_{0|0}{b}_0& {\hat{b}}_{0|0}=b_0\end{array}$

$U_{0|0}=P_0^{\mathrm{xb}}{P}_0^{b-1}$

$\begin{array}{cc}{P}_{0|0}^x=P_0^x-U_{0|0}{P}_0^b{U}_{0|0}^T& P_{0|0}^b=P_0^h\end{array}$

整个滤波过程便可以从第一步开始不断估计出下一步，一直持续下去，整个滤波估计过程得以实现。 

Claims (1)

1.基于非线性未知随机偏差的二阶段容积卡尔曼滤波方法，其特征在于：

系统建模，给出如下非线性系统动态模型

x_k＝f_k-1(x_k-1)+w_k,k-1

z_k＝h_k(x_k)+v_k

其中k≥1是时刻指数，x_k∈R^n×1表示系统状态，R^n×1为n×1维列向量全集，z_k∈R^n×1是测量值列向量，f_k-1(·)以及h_k(·)都是可微函数；初始状态x₀服从为均值，P₀为方差且独立于w_k和v_k随机变量；w_k∈R^n×1和v_k∈R^m×1都是均值为零的高斯白噪声，其中 $\begin{array}{ccc}E\left[w_k{w}_j^T\right]=Q_k{\mathrm{\δ}}_{\mathrm{ij}}& E\left[v_k{v}_j^T\right]=R_k{\mathrm{\δ}}_{\mathrm{ij}}& E\left[w_k{v}_j^T\right]=0\end{array},$ δ_ij是克罗内克脉冲函数；

在容积卡尔曼中，n维随机变量x_k以为均值P_k为方差的随机变量，它可以被容积点近似为其中权重值w_i为 $w_i=\frac{1}{m}(i=\mathrm{1,2},...m;m=2n);$

当非线性系统信息不完全时，离散两阶容积卡尔曼滤波的分析方程如下：

$\left\{\begin{array}{c}{\hat{x}}_{k|k-1}^{*}={\hat{x}}_{k|k-1}+U_{k|k-1}{b}_{k|k-1}\\ {\hat{x}}_{k|k}={\hat{x}}_{k|k}+U_{k|k}{b}_{k|k}\\ P_{k|k-1}^{x*}=P_{k|k-1}^x+U_{k|k-1}{P}_{k|k-1}^b{U}_{k|k-1}^T\\ P_{k|k}^{x*}=P_{k|k}^x+U_{k|k}{P}_{k|k}^b{U}_{k|k}^T\end{array}\right.$

其中和b_k为两阶容积卡尔曼的状态向量，修正无偏差估计滤波器和偏差部分分别如下：其中下标k|k表示通过前k时刻的观测值所得到的状态估计值，同理k|k-1表示的是根据前k-1个时刻的观测值得到k时刻的状态估计值；

修正无偏差部分：

步骤1.计算不含有未知偏差部分的无偏差估计一步预测一步预测误差方差一步预测误差协方差

1).计算容积点一步递归x_i,k|k-1

x_i,k|k-1＝f(x_i,k-1|k-1)   (1)

2).根据m个容积点的采样来求得一步预测容积点近似估计值

${\hat{x}}_{k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i{x}_{i,k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{x}_{i,k|k-1}---\left(2\right)$

3).一步预测误差估计方差，其中代表高斯白噪w_k-1的方差

$\begin{array}{c}{P}_{k|k-1}^x=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i(x_{i,k|k-1}-{\hat{x}}_{k|k-1}){(x_{i,k|k-1}-{\hat{x}}_{k|k-1})}^T+Q_{k-1}^x\\ =\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}(x_{i,k|k-1}-{\hat{x}}_{k|k-1}){(x_{i,k|k-1}-{\hat{x}}_{k|k-1})}^T+Q_{k-1}^x\end{array}---\left(3\right)$

其中右上角标x表明是不含偏差部分的方差计算，包含^代表估计值；

4).容积点一步观测值递归

z_i,k|k-1＝h(x_i,k|k-1)   (4)

5).通过m个容积点近似一步预测观测估计值

${\hat{z}}_{k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i{z}_{i,k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{z}_{i,k|k-1}---\left(5\right)$

6).得到新息z_k以后，根据上一步得到的一步预测观测值，求其误差

${\mathrm{\ϵ}}_k^x=z_k-{\hat{z}}_{k|k-1}=z_k-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{z}_{i,k|k-1}---\left(6\right)$

7).观测值预测方差

$\begin{array}{c}{P}_{\mathrm{zz},k|k-1}^x=E\left[{\mathrm{\ϵ}}_k^x{\mathrm{\ϵ}}_k^{\mathrm{xT}}\right]\\ =\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i(z_{i,k|k-1}-{\hat{z}}_{k|k-1}){(z_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T+R_k\\ =\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}(z_{i,k|k-1}-{\hat{z}}_{k|k-1}){(z_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T+R_k\end{array}---\left(7\right)$

8).一步预测状态与观测协方差

$\begin{array}{c}{P}_{\mathrm{xz},k|k-1}^x=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i(x_{i,k|k-1}-{\hat{x}}_{k|k-1}){(z_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T\\ =\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}(x_{i,k|k-1}-{\hat{x}}_{k|k-1}){(z_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T\end{array}---\left(8\right)$

步骤2.计算不含有未知偏差部分的无偏差估计增益阵估计值和估计误差方差阵P_k|k

1).根据上两步得到一步预测增益值

$\begin{array}{c}{K}_k^x=P_{k|k-1}^x{N}_k^{\mathrm{xT}}{P}_{\mathrm{zz},k|k-1}^{x-1}\\ =P_{\mathrm{xz},k|k-1}^x{P}_{\mathrm{zz},k|k-1}^{x-1}\end{array}---\left(9\right)$

其中由 $N_k^x={\left[P_{k|k-1}^{x-1}{P}_{k|k-1}^x\right]}^T$

2).根据上一步的预测增益值可以得到状态估计

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k^x{\mathrm{\ϵ}}_k^x---\left(10\right)$

3).根据(3)、(7)、(10)状态估计误差方差

$P_{k|k}^x=P_{k|k-1}^x-K_k^x{P}_{\mathrm{zz},k|k-1}^x{K}_k^{\mathrm{xT}}---\left(11\right)$

偏差估计部分：

步骤3.计算含有未知偏差部分的，未知偏差一步预测b_k|k-1偏差一步预测方差偏差预测误差协方差以及

1).一步预测观测值误差

$\begin{array}{c}{\mathrm{\ϵ}}_k^b=z_k-{\hat{z}}_{k|k-1}-N_k^b{b}_{k|k-1}\\ ={\mathrm{\ϵ}}_k^x-N_k^b{b}_{k|k-1}\end{array}---\left(12\right)$

其中右上角标b表示的是含偏差部分，是由之后的耦合方程

$N_k^b={\left[P_{k|k-1}^{b-1}{P}_{\mathrm{xz},k|k-1}^b\right]}^T$ 求出的耦合系数；

2).一步预测观测值误差协方差

$\begin{array}{c}{P}_{\mathrm{zz},k|k-1}^b=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i(z_{i,k|k-1}-{\hat{z}}_{k|k-1}){(z_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T+R_k+N_k^b{P}_{k|k-1}^b{N}_k^{\mathrm{bT}}\\ =\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\left(z_{i,k|k-1}-{\hat{z}}_{k|k-1}\right){(z_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T+R_k+N_k^b{P}_{k|k-1}^b{N}_k^{\mathrm{bT}}\end{array}---\left(13\right)$

3).一步预测观测误差协方差

$\begin{array}{c}{P}_{\mathrm{xz},k|k-1}^b=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i(x_{i,k|k-1}-{\hat{x}}_{k|k-1}){(z_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T+D_{k-1}{P}_{k|k-1}^b{N}_k^{\mathrm{bT}}\\ =\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\left(x_{i,k|k-1}-{\hat{x}}_{k|k-1}\right){(z_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T-D_{k-1}{P}_{k|k-1}^b{N}_k^{\mathrm{bT}}\end{array}---\left(14\right)$

$P_{k|k-1}^{\mathrm{xb}}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}(x_{i,k|k-1}-{\hat{x}}_{k|k-1}){(b_k-{\hat{b}}_{k|k-1})}^T---\left(15\right)$

4).状态偏差一步预测

b_k|k-1＝b_k-1|k-1   (16)

5).关于误差偏差b的一步偏差预测误差方差阵

$P_{k|k-1}^b=P_{k-1|k-1}^b+Q_{k-1}^b---\left(17\right)$

其中是上一步的估计误差方差，是一步状态偏差噪声的方差值

6).状态偏差值迭代

$\begin{array}{c}{b}_{k|k}=b_{k-1|K-1}+K_k^b({\mathrm{\ϵ}}_k^x-N_k^b{b}_{k-1|k-1})\\ =b_{k|k-1}+K_k^b{\mathrm{\ϵ}}_k^b\end{array}---\left(18\right)$

其中为耦合方程 $N_k^b={\left[P_{k|k-1}^{b-1}{P}_{\mathrm{xz},k|k-1}^b\right]}^T$ 所求的值

7).通过(16)式算出的一步偏差预测方差阵求得

$P_{k|k}^b=(I-K_k^b{N}_k^b)P_{k|k-1}^b---\left(19\right)$

其中为耦合方程 $N_k^b={\left[P_{k|k-1}^{b-1}{P}_{\mathrm{xz},k|k-1}^b\right]}^T$ 所求的值

步骤4.计算含有未知偏差部分的，偏差增益阵和偏差估计误差方差阵以及偏差一步增益U_k|k-1和偏差增益

1).计算关于状态误差增益系数阵

$\begin{array}{c}{K}_k^b=P_{k|k-1}^b{N}_k^{\mathrm{bT}}{[\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i(z_{i,k|k-1}-{\hat{z}}_{k|k-1}){(z_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T+R_k+N_k^b{P}_{k|k-1}^b{N}_k^{\mathrm{bT}}]}^{-1}\\ =P_{k|k-1}^b{N}_k^{\mathrm{bT}}{(P_{\mathrm{zz},k|k-1}^x+N_k^b{P}_{k|k-1}^b{N}_k^{\mathrm{bT}})}^{-1}\\ =P_{k|k-1}^b{N}_k^{\mathrm{bT}}{\left(P_{\mathrm{zz},k|k-1}^b\right)}^{-1}\end{array}---\left(20\right)$

2).耦合方程如下

$N_k^x={\left[P_{k|k-1}^{x-1}{P}_{\mathrm{xz},k|k-1}^x\right]}^T---\left(21a\right)$

$N_k^b={\left[P_{k|k-1}^{b-1}{P}_{\mathrm{xz},k|k-1}^b\right]}^T---\left(21b\right)$

$U_{k|k-1}=P_{k|k-1}^{\mathrm{xb}}{P}_{\mathrm{xz},k|k-1}^{b-1}---\left(21c\right)$

$U_{k|k}=U_{k|k-1}-K_k^x{N}_k^b---\left(21d\right)$

步骤5.根据以上四步计算未知偏差非线性系统的一步预测方差和一步预测误差协方差和最终目标状态的最优线性估计及其误差协方差

$\left\{\begin{array}{c}{\hat{x}}_{k|k-1}^{*}={\hat{x}}_{k|k-1}+U_{k|k-1}{b}_{k|k-1}\\ {\hat{x}}_{k|k}={\hat{x}}_{k|k}+U_{k|k}{b}_{k|k}\\ P_{k|k-1}^{x*}=P_{k|k-1}^x+U_{k|k-1}{P}_{k|k-1}^b{U}_{k|k-1}^T\\ P_{k|k}^{x*}=P_{k|k}^x+U_{k|k}{P}_{k|k}^b{U}_{k|k}^T\end{array}\right.$

将修正无偏差部分所算出来的和含偏差部分所求出来的U_k|k-1,b_k|k-1,U_k|k,b_k|k,带入到离散两阶容积卡尔曼滤波的分析方程中，便可以得到由上一步状态估计和状态估计偏差在得到信息之后得到下一步的状态估计和状态估计偏差；

所以当知道关于偏差估计部分的初始条件：

$\begin{array}{cc}{\hat{x}}_{0|0}=x_0-U_{0|0}{b}_0& {\hat{b}}_{0|0}=b_0\end{array}$

$U_{0|0}=P_0^{\mathrm{xb}}{P}_0^{b-1}$

$\begin{array}{cc}{P}_{0|0}^x=P_0^x-U_{0|0}{P}_0^b{U}_{0|0}^T& P_{0|0}^b=P_0^h\end{array}$

整个滤波过程便可以从第一步开始不断估计出下一步，一直持续下去，整个滤波估计过程得以实现。