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

权利要求书

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

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

x_k＝f_k-1(x_k-1)+w_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都是均值为零的高斯白噪声，其中δ_ij是克罗内克脉冲函数；

在容积卡尔曼中，n维随机变量x_k以为均值P_k为方差的随机变量，它可以被容积点近似为其中权重值w_i为

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

$\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|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}{\Σ}}{w}_i{x}_{i,k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\Σ}}{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}{\Σ}}{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}{\Σ}}(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-1的情况；

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}{\Σ}}{w}_i{z}_{i,k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\Σ}}{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}{\Σ}}{z}_{i,k|k-1}---\left(6\right)$

7).观测值预测方差

$\begin{array}{c}{P}_{zz,k|k-1}^x=E\[{\mathrm{\ϵ}}_k^x{\mathrm{\ϵ}}_k^{xT}\]\\ =\underset{i=1}{\overset{m}{\Σ}}{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}{\Σ}}(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}_{xz,k|k-1}^x=\underset{i=1}{\overset{m}{\Σ}}{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}{\Σ}}(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).根据步骤1第7)步和第8)步得到无偏差增益

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

其中由

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

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

3).根据步骤1第3)步中公式(3)、步骤1第7)步中公式(7)、和步骤2第2)步中公式(10)可得到估计误差方差

$P_{k|k}^x=P_{k|k-1}^x-K_k^x{P}_{zz,k|k-1}^x{K}_k^{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)$

其中表示含偏差部分的方差，是由之后的耦合方程

求出的耦合关系；

2).偏差观测值预测方差

$\begin{array}{c}{P}_{zz,k|k-1}^b=\underset{i=1}{\overset{m}{\Σ}}{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^{bT}\\ =\frac{1}{m}\underset{i=1}{\overset{m}{\Σ}}(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^{bT}\end{array}---\left(13\right)$

3).偏差一步预测交叉协方差

$\begin{array}{c}{P}_{xz,k|k-1}^b=\underset{i=1}{\overset{m}{\Σ}}{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^{bT}\\ =\frac{1}{m}\underset{i=1}{\overset{m}{\Σ}}(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^{bT}\end{array}---\left(14\right)$

$P_{k|k-1}^{xb}=\frac{1}{m}\underset{i=1}{\overset{m}{\Σ}}(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)$

其中为耦合方程所求的值

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

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

其中为耦合方程所求的值

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

1).计算关于偏差增益

$\begin{array}{c}{K}_k^b=P_{k|k-1}^b{N}_k^{bT}{\[\underset{i=1}{\overset{m}{\Σ}}{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^{bT}\]}^{-1}\\ =P_{k|k-1}^b{N}_k^{bT}{(P_{zz,k|k-1}^x+N_k^b{P}_{k|k-1}^b{N}_k^{bT})}^{-1}\\ =P_{k|k-1}^b{N}_k^{bT}{\left(P_{zz,k|k-1}^b\right)}^{-1}\end{array}---\left(20\right)$

2).耦合方程如下

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

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

$U_{k|k-1}=P_{k|k-1}^{xb}{P}_{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.根据步骤1-4计算未知偏差非线性系统的一步预测和一步预测误差协方差和最终目标状态的估计及其误差协方差

$\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^{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^b\end{array}$

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