CN104283529B - 未知测量噪声方差的平方根高阶容积卡尔曼滤波方法 - Google Patents

Abstract

本发明属于非线性系统的滤波领域，特别涉及一种处理含有未知测量噪声方差的平方根高阶容积卡尔曼滤波方法。该方法用于处理未知测量噪声方差的平方根高阶容积Kalman滤波，以HCKF为基础，首先利用QR分解，Cholesky因子更新和高效最小二乘法等矩阵分解技术，提高了滤波方法的运行效率和数值稳定性。并在SHCKF基础上，通过采用Sage‑Husa估计器实时估算量测噪声的统计特性，有效解决了测量噪声方差位置的非线性系统滤波问题。

Description

未知测量噪声方差的平方根高阶容积卡尔曼滤波方法

技术领域

本发明属于非线性系统的滤波领域，特别涉及一种处理含有未知测量噪声方差的平方根高阶容积卡尔曼滤波方法。

背景技术

在诸多领域中，系统状态的动态估计问题一直是人们关注的焦点。线性高斯系统的状态估计问题，一般采用Kalman滤波方法。但是在处理实际问题时，比如目标跟踪、导航定位以及视频监控等，系统的状态方程或测量方程通常表现出较强的非线性特征。因此，研究非线性系统的状态估计即非线性滤波问题具有重要的理论意义和实际应用价值。扩展Kalman滤波(EKF)是一种最直接、最简单的非线性滤波方法。EKF方法通过对非线性函数的泰勒转换开始进行一阶线性化截断，将非线性问题转化为线性问题，然后利用Kalman滤波方法进行处理。对于一般的非线性系统而言，EKF不能保证其收敛，状态估计的误差较大。无迹Kalman滤波是一类用采样策略逼近非线性分布的方法。其核心是通过一种非线性变换——U变化来进行非线性模型的状态与误差协方差的递推和更新。与EKF不同。UKF不是对非线性模型做近似，而是对状态的概率密度函数做近似，因而能获得比EKF更高的估计精度。为了解决UKF应对高维状态效果不理想的问题，有人提出了3阶容积Kalman滤波(CKF)。该方法利用球形积分准则和径向积分准则优化了UKF中的sigma点采样策略和权重分配，改善了滤波性能。为了进一步估计精度，又有人提出了高阶容积Kalman滤波方法，它能获得与粒子滤波相当的性能，但在计算开销上小于后者。然而，标准的UKF、CKF和HCKF算法由于数值计算舍入误差、可观测性弱(初值误差较大)和观测噪声大等因素影响，可能引起误差协方差矩阵负定，而导致滤波器不稳定，甚至不能工作。为此，众多学者通过采用误差协方差阵的平方根代替协方差阵参加递推运算，提出一系列平方根UKF和平方根CKF算法，较好地解决了滤波器不稳定问题。

然而，上述非线性滤波应用的一个先决条件是己知噪声的统计特性，由于实际系统噪声统计特性往往具有不确定性，这就会导致滤波性能下降。对实际应用系统而言，量测噪声方差总是时变未知的，这是因为量测系统受到内外部各种因素的干扰，包括测量误差和环境扰动。也就是说，噪声统计特性未知或者知道的不确切，此时需要在滤波过程中进行估计。

发明内容

针对上述的这些问题，一种以处理未知测量噪声方差的平方根高阶容积Kalman滤波方法应运而生，该方法以HCKF为基础，首先利用QR分解，Cholesky因子更新和高效最小二乘法等矩阵分解技术，提高了滤波算法的运行效率和数值稳定性。接着，在SHCKF基础上，通过采用Sage-Husa估计器实时估算量测噪声的统计特性，有效解决了测量噪声方差未知的非线性系统滤波问题。

本发明是SHCKF的改进形式，包括计算预测状态估计值以及计算预测误差方差阵的平方根S(k|k-1)、计算测量与测量计算互协方差阵P_xz(k|k-1)、计算噪声方差计算新息协方差阵的平方根S_zz(k|k-1)、计算增益阵K(k)、估计状态及协方差阵的平方根S(k|k)。具体内容如下：

步骤1滤波系统初始化：S₀＝chol(p₀)、

步骤2预测过程，具体包括：

(2.1)计算高阶容积点：x_i(k-1|k-1)

(2.2)计算传播后的容积点

(2.3)状态预测估计值

(2.4)计算预测误差方差阵的平方根S(k|k-1)

步骤3更新过程

(3.1)计算容积点(i＝0,1,…2n²)，x_i(k|k-1)

(3.2)计算传播后的容积点z_i(k|k-1)

(3.3)测量预测

(3.4)计算互协方差阵P_xz(k|k-1)和增益阵K(k)(下标xz表示状态和测量值的互协方差)

(3.5)在线估计v(k)的方差

(3.6)计算新息方差阵的平方根S_zz(k|k-1)

(3.7)计算增益阵K(k)

(3.8)计算估计状态及其协方差的平方根S(k|k)。

本发明的有益效果：首先和HCKF相比，提高了滤波算法的运行效率和数值稳定性。并且在HCKF的基础上通过采用Sage-Husa估计器实时估算测量噪声的统计特性，有效解决了测量噪声方差未知的非线性系统滤波问题。

附图说明

图1为未知测量噪声方差的SHCKF。

具体实施方式

下面结合附图和实施例对本发明做进一步说明。

参照图1，设非线性动态系统的状态空间模型为：

x(k+1)＝f(x(k))+w(k)

z(k)＝h(x(k))+v(k)

上式中，x(k)∈Rⁿ为目标状态，z(k)∈R^m表示测量值；f:Rⁿ→Rⁿ为非线性状态演化过程，h:Rⁿ→R^m为相应的非线性测量映射；过程噪声w(k)∈Rⁿ是均值为零的高斯白噪声，其方差为Q(k)；测量噪声v(k)∈R^m是均值为零的高斯白噪声，但方差R(k)时变未知。

假设系统模型中过程噪声与测量噪声互不相关。系统的初始状态x(0)均值为x₀，方差为P₀，且独立于w(k)和v(k)。

下面，基于系统模型，详述未知测量噪声方差的SHCKF的具体实施步骤：

步骤1设置滤波初始条件：S₀＝chol(P₀)，

步骤2预测过程。

1)计算高阶容积点(i＝0,1,…,2n²)

其中，S(k-1|k-1)为方差P(k-1|k-1)的Cholesky分解因子，点集{ξ_i}由下式确定：

其中，e_i为n阶单位向量，且其第i个元素为1。点集和由下式给出：

而相应的权值系数ω_i为：

2)计算传播后的容积点

3)状态预测估计值

4)计算预测误差方差阵的平方根

其中，qr(·)表示对矩阵进行QR分解，表示矩阵Q(k-1)的Cholesky分解因子。

步骤3更新过程

1)计算容积点(i＝0,1,…2n²)

2)计算传播后的容积点

z_i(k|k-1)＝h(x_i(k|k-1)) (6)

3)测量预测

4)计算互协方差阵与增益阵

5)在设计状态估计算法时需要在线估计v(k)的方差

其中：

P_xz(k|k-1)＝P(k|k-1)H^T(k,k-1)

d(k)＝(1-b)/(1-b^k+1)b为遗忘因子，其取值范围通常为0.95<b<0.99。b用来限制滤波器的记忆长度，当噪声变化较快时，b应取值偏大，反之则应取值偏小。为测量新息，H(k,k-1)为线性化后的观测矩阵。

即：

6)计算新息协方差阵的平方根

7)计算增益阵K(k)

8)估计状态及其协方差阵的平方根

Claims (1)

1.未知测量噪声方差的平方根高阶容积卡尔曼滤波方法，其特征在于：

设非线性动态系统的状态空间模型为：

x(k+1)＝f(x(k))+w(k)

z(k)＝h(x(k))+v(k)

上式中，x(k)∈Rⁿ为目标状态，z(k)∈R^m表示测量值；f:Rⁿ→Rⁿ为非线性状态演化过程，h:Rⁿ→R^m为相应的非线性测量映射；过程噪声w(k)∈Rⁿ是均值为零的高斯白噪声，其方差为Q(k)；测量噪声v(k)∈R^m是均值为零的高斯白噪声，但方差R(k)时变未知；

假设系统模型中过程噪声与测量噪声互不相关；系统的初始状态x(0)均值为x₀，方差为P₀，且独立于w(k)和v(k)；

基于上述系统模型，该方法具体步骤是：

步骤1设置滤波初始条件：S₀＝chol(P₀)，

步骤2预测过程；

1)计算高阶容积点

$x_i(k-1|k-1)=S(k-1|k-1){\mathrm{\&xi;}}_i+\hat{x}(k-1|k-1)---\left(1\right)$

其中，S(k-1|k-1)为方差P(k-1|k-1)的Cholesky分解因子，点集{ξ_i}由下式确定：

${\mathrm{\&xi;}}_i=\left\{\begin{array}{cc}{\&lsqb;0,...,0\&rsqb;}^T,& i=0\\ \sqrt{n+2}\&CenterDot;{\mathrm{\&epsiv;}}_i^{+},& i=1,...,\frac{n(n-1)}{2}\\ -\sqrt{n+2}\&CenterDot;{\mathrm{\&epsiv;}}_{i-n(n-1)/2}^{+},& i=\frac{n(n-1)}{2}+1,...,n(n-1)\\ \sqrt{n+2}\&CenterDot;{\mathrm{\&epsiv;}}_{i-n(n-1)}^{-},& i=n(n-1)+1,...,\frac{3n(n-1)}{2}\\ -\sqrt{n+2}\&CenterDot;{\mathrm{\&epsiv;}}_{i-3n(n-1)/2}^{-},& i=\frac{3n(n-1)}{2}+1,...,2n(n-1)\\ \sqrt{n+2}\&CenterDot;e_{i-2n(n-1)},& i=2n(n-1)+1,...,n(2n-1)\\ -\sqrt{n+2}\&CenterDot;e_{i-n(2n-1)},& i=n(2n-1)+1,...,2n^2\end{array}\right.$

其中，e_i为n阶单位向量，且其第i个元素为1；点集和由下式给出：

$\left\{\begin{array}{cc}{\mathrm{\&epsiv;}}_j^{+}\stackrel{\mathrm{\&Delta;}}{=}\sqrt{\frac{1}{2}}\&CenterDot;(e_k+e_l):k<l,& k,l=1,...,n\\ {\mathrm{\&epsiv;}}_j^{-}\stackrel{\mathrm{\&Delta;}}{=}\sqrt{\frac{1}{2}}\&CenterDot;(e_k-e_l):k<l,& k,l=1,...,n\end{array}\right.$

而相应的权值系数ω_i为：

${\mathrm{\&omega;}}_i=\left\{\begin{array}{cc}\frac{2}{n+2},& i=0\\ \frac{1}{{(n+2)}^2},& i=1,...,2n(n-1)\\ \frac{4-n}{2{(n+2)}^2},& i=2n(n-1)+1,...,2n^2\end{array}\right.$

2)计算传播后的容积点

$x_i^{*}\left(k\right|k-1)=f\left(x_i\right(k-1|k-1\left)\right)---\left(2\right)$

3)状态预测估计值

$\hat{x}\left(k\right|k-1)=\underset{i=0}{\overset{2n^2}{\&Sigma;}}{\mathrm{\&omega;}}_i{x}_i^{*}\left(k\right|k-1)---\left(3\right)$

4)计算预测误差方差阵的平方根

$\begin{array}{c}S\left(k\right|k-1)=qr\&lsqb;\sqrt{{\mathrm{\&omega;}}_0}\left(x_0^{*}\right(k|k-1)-\hat{x}(k|k-1\left)\right)\\ ...\sqrt{{\mathrm{\&omega;}}_{2n^2}}\left(x_{2n^2}^{*}\right(k|k-1)-\hat{x}(k|k-1\left)\right),\sqrt{Q(k-1)}\&rsqb;\end{array}---\left(4\right)$

其中，qr(·)表示对矩阵进行QR分解，表示矩阵Q(k-1)的Cholesky分解因子；

步骤3更新过程

1)计算容积点

$S_i\left(k\right|k-1)=S\left(k\right|k-1){\mathrm{\&xi;}}_i+\hat{x}\left(k\right|k-1)---\left(5\right)$

2)计算传播后的容积点

z_i(k|k-1)＝h(x_i(k|k-1)) (6)

3)测量预测

$\hat{z}\left(k\right|k-1)=\underset{i=0}{\overset{2n^2}{\&Sigma;}}{\mathrm{\&omega;}}_i{z}_i\left(k\right|k-1)---\left(7\right)$

4)计算互协方差阵与增益阵

$\begin{array}{c}{P}_{xz}\left(k\right|k-1)=\underset{i=0}{\overset{2n^2}{\&Sigma;}}{\mathrm{\&omega;}}_i\&lsqb;x_i\left(k\right|k-1)-\hat{x}\left(k\right|k-1)\&rsqb;\\ \&times;{\&lsqb;z_i\left(k\right|k-1)-\hat{z}\left(k\right|k-1)\&rsqb;}^T\end{array}---\left(8\right)$

5)在设计状态估计算法时需要在线估计v(k)的方差

$\begin{array}{c}\hat{R}\left(k\right)=\&lsqb;1-d(k-1)\&rsqb;\hat{R}(k-1)+d(k-1)\&lsqb;\tilde{z}\left(k\right)\\ \&times;{\hat{z}}^T\left(k\right)-H(k,k-1)P\left(k\right|k-1)H^T(k,k-1)\&rsqb;\end{array}---\left(9\right)$

其中：

P_xz(k|k-1)＝P(k|k-1)H^T(k,k-1)

$H(k,k-1)=P_{xz}^T\left(k\right|k-1)/\left(S^T\right(k|k-1\left)S\right(k|k-1\left)\right)$

d(k)＝(1-b)/(1-b^k+1)，b为遗忘因子，b用来限制滤波器的记忆长度；为测量新息，H(k,k-1)为线性化后的观测矩阵；

即：

$\begin{array}{c}\hat{R}\left(k\right)=\&lsqb;1-d(k-1)\&rsqb;\hat{R}(k-1)+d(k-1)\\ \&times;\&lsqb;\tilde{z}\left(k\right){\hat{z}}^T\left(k\right)-P_{xz}^T\left(k\right|k-1)\\ \&times;{\left(S^T\right(k,k-1\left)S\right(k,k-1\left)\right)}^{-1}\\ \&times;P_{xz}\left(k\right|k-1)\&rsqb;\end{array}---\left(10\right)$

6)计算新息协方差阵的平方根

$\begin{array}{c}{S}_{zz}\left(k\right|k-1)=qr\&lsqb;\sqrt{{\mathrm{\&omega;}}_0}\left(z_0\right(k|k-1)-\hat{z}(k|k-1\left)\right)\\ ...\sqrt{{\mathrm{\&omega;}}_{2n^2}}\left(z_{2n^2}\right(k|k-1)-\hat{z}(k|k-1\left)\right)\\ \sqrt{\hat{R}\left(k\right)}\&rsqb;\end{array}---\left(11\right)$

7)计算增益阵K(k)

$K\left(k\right)=\&lsqb;P_{xz}\left(k\right|k-1)/S_{zz}^T\left(k\right|k-1)\&rsqb;/S_{zz}\left(k\right|k-1)---\left(12\right)$

8)估计状态及其协方差阵的平方根

$\left\{\begin{array}{c}\hat{x}\left(k\right|k)=\hat{x}\left(k\right|k-1)+K\left(k\right)\&lsqb;z\left(k\right)-\hat{z}\left(k\right|k-1)\&rsqb;\\ S\left(k\right|k)=cholupdate\&lsqb;S\left(k\right|k-1),\\ K\left(k\right)S_{zz}\left(k\right|k-1),-1\&rsqb;\end{array}\right.---\left(13\right).$