非线性系统基于分散式容积信息滤波的目标跟踪方法

摘要

本发明属于目标跟踪领域，主要涉及一种非线性系统基于分散式容积信息滤波的目标跟踪方法。现有的容积卡尔曼的非线性系统目标跟踪方法是在过程噪声与测量噪声不相关及各测量噪声也互不相关的假设前提下进行的。这就大大限制了它的使用范围。本发明推导了噪声相关的扩展卡尔曼信息滤波，并在时间更新与测量更新这两个过程中嵌入容积卡尔曼信息滤波，也就解决了噪声相关的问题，使得本发明的方法实用性大大增强，另外本发明是基于分散式的，利用矩阵对角化原理，很大程度上降低了矩阵的维数，避免了高维带来的维数灾难问题。

主权项

非线性系统基于分散式容积信息滤波的目标跟踪方法，其特征在于：针对多传感器目标系统建立模型，包括两个方程，状态方程和观测方程，分别如下所示：x_k＝f_k‑1(x_k‑1)+w_k,k‑1        (1)z_i,k＝h_i,k(x_k)+v_i,k        (2)其中，k是时间指标，i表示第i个传感器，i＝1,2,...N；x_k∈R^n×1是系统状态向量，表示第i个观测向量；f_k‑1:R^n×1→R^n×1，均为已知的非线性方程；过程噪声w_k,k‑1和观测噪声v_i,k都为零均值的高斯白噪声，它们的方差分别为Q_k,k‑1和R_i,k且满足$E\left\{w_{k,k-1}{w}_{k,k-1}^T\right\}=Q_{k,k-1},E\left\{v_{i,k}{v}_{j,l}^T\right\}=R_{ij,k}{\delta }_{kl},E\left\{w_{k,k-1}{v}_{i,l}^T\right\}=D_{i,k}{\delta }_{kl}$其中R_ij,k为第i个传感器观测噪声与第j个传感器观测噪声的协方差矩阵，D_i,k为过程噪声与第i个传感器观测噪声的互协方差矩阵，δ_k,l为脉冲函数，即k＝l时，δ_k,l＝1，k≠l时，δ_k,l＝0；初始状态为x₀及协方差矩阵P_0|0满足：$E\left\{x_0\right\}={\hat{x}}_{0|0},E\left\{\right[x_0-{\hat{x}}_{0|0}\left]{[x_0-{\hat{x}}_{0|0}]}^T\right\}=P_{0|0}---\left(3\right)$运用集中式扩维，将N个观测方程融合成一个观测方程，也就是：z_k＝h_k(x_k)+v_k          (4)其中，$z_k={[z_{1,k}^T,z_{2,k}^T,...,z_{N,k}^T]}^T\in R^{{\Sigma }_{i=1}^N{m}_i\times 1}---\left(5\right)$$h_k\left(x_k\right)={[h_{1,k}^T\left(x_k\right),h_{2,k}^T\left(x_k\right),...,h_{N,k}^T\left(x_k\right)]}^T\in R^{{\Sigma }_{i=1}^N{m}_i\times 1}---\left(6\right)$$v_k={[v_{1,k}^T,v_{2,k}^T,...,v_{N,k}^T]}^T\in R^{{\Sigma }_{i=1}^N{m}_i\times 1}---\left(7\right)$由此可以得到：$D_k=[D_{1,k}^T,D_{2,k}^T,...,D_{N,k}^T]\in R^{n\times {\Sigma }_{i=1}^N{m}_i}---\left(8\right)$针对式(1)(4)所描述的多传感器系统模型，给出如下迭代算法，具体包括两个模块：时间更新和测量更新；1.时间更新步骤1.1分别计算k‑1时刻第i个容积点X_{i,k‑1|k‑1},k‑1时刻第i个传播容积点和k‑1时刻一步状态预测首先，假设k‑1时刻的状态估计和它的协方差矩阵P_k‑1|k‑1已知.分解P_k‑1|k‑1有：$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T---\left(10\right)$其中S_k‑1|k‑1称为k‑1时刻开方值；其次，从(11)式计算传播容积点i＝1,2,…,M＝2n$X_{i,k|k-1}^{*}=f_{k-1}\left(X_{i,k-1|k-1}\right)---\left(11\right)$其中，$X_{i,k-1|k-1}=S_{k-1|k-1}{\xi }_i+{\hat{x}}_{k-1|k-1}---\left(12\right)$并且这里,[1]_i是点集合[1]的第i个列向量；最后，计算k‑1时刻状态的一步预测：${\hat{x}}_{k|k-1}=\frac{1}{M}\underset{i=1}{\overset{M}{\Sigma }}{X}_{i,k|k-1}^{*}---\left(13\right)$步骤1.2根据下式计算k‑1时刻一步开方S_k|k‑1；$S_{k|k-1}=Tria\left(\left[\begin{array}{cc}{\gamma }_{k|k-1}^{*}& S_{Q_{k,k-1}}\end{array}\right]\right)---\left(14\right)$这里Tria表示QR分解，将分解得到的上三角矩阵的转置赋给S_k|k‑1，是Q_k,k‑1的开方根，即：并且${\gamma }_{k|k-1}^{*}=\frac{1}{\sqrt{M}}\left[\begin{array}{cccc}{X}_{1,k|k-1}^{*}-{\hat{x}}_{k|k}& X_{2,k|k-1}^{*}-{\hat{x}}_{k|k}& \mathrm{...}& X_{M,k|k-1}^{*}-{\hat{x}}_{k|k}\end{array}\right]---\left(15\right)$$Y_{k|k-1}=P_{k|k-1}^{-1}{\left({\gamma }_{k|k-1}{\gamma }_{k|k-1}^T\right)}^{-1}$步骤1.3使用下面的式子得到k‑1时刻一步信息矩阵Y_k|k‑1；令${\gamma }_{k|k-1}=\frac{1}{\sqrt{M}}\left[\begin{array}{cccc}{X}_{1,k|k-1}-{\hat{x}}_{k|k-1}& X_{2,k|k-1}-{\hat{x}}_{k|k-1}& \mathrm{...}& X_{M,k|k-1}-{\hat{x}}_{k|k-1}\end{array}\right]---\left(16\right)$然后利用即可得到一步信息矩阵Y_k|k‑1；步骤1.4使用式(17)式计算k‑1时刻一步预测信息状态向量$\{\begin{array}{c}{\hat{y}}_{k|k-1}=P_{k|k-1}^{-1}{\hat{x}}_{k|k-1}=\frac{1}{M}\left(Y_{k|k-1}\underset{i=1}{\overset{M}{\Sigma }}{X}_{i,k|k-1}^{*}\right)\\ {\upsilon }_k=z_k-{\hat{z}}_{k|k-1}=z_k-\frac{1}{M}\underset{i=1}{\overset{M}{\Sigma }}{Z}_{i,k|k-1}\end{array}---\left(17\right)$2.测量更新步骤2.1分别计算k‑1时刻第i个一步容积点X_i,k|k‑1,k‑1时刻第i个一步传播容积点Z_i,k|k‑1和k‑1时刻观测一步预测首先，计算k‑1时刻一步容积点X_i,k|k‑1，如下式所示：$X_{i,k|k-1}=S_{k|k-1}{\zeta }_i+{\hat{x}}_{k|k-1}---\left(18\right)$进而可利用下式计算k‑1时刻一步传播容积点，Z_i,k|k‑1＝h_k(X_i,k|k‑1)          (19)然后，利用式(20)计算k‑1时刻观测一步预测${\hat{z}}_{k|k-1}=\frac{1}{M}\underset{i=1}{\overset{M}{\Sigma }}{Z}_{i,k|k-1}---\left(20\right)$步骤2.2计算k‑1时刻互协方差矩阵首先，根据下式计算k‑1时刻开方新息协方差矩阵$S_{\tilde{z}\tilde{z},k|k-1}=Tria\left(\left[\begin{array}{cc}{\zeta }_{k|k-1}^{*}& S_{{\bar{R}}_k}\end{array}\right]\right)---\left(21\right)$其中，表示R_k的开方；${\zeta }_{k|k-1}=\frac{1}{\sqrt{M}}\left[\begin{array}{cccc}{Z}_{1,k|k-1}-{\hat{z}}_{k|k}& Z_{2,k|k-1}-{\hat{z}}_{k|k}& \mathrm{...}& Z_{M,k|k-1}-{\hat{z}}_{k|k}\end{array}\right]$然后，计算k‑1时刻互协方差矩阵$P_{\tilde{x}\tilde{z},k|k-1}={\gamma }_{k|k-1}{\zeta }_{k|k-1}^T---\left(22\right)$步骤2.3下面计算k时刻相关信息矩阵I_k和信息状态增益i_k；由于$D_k\in R^{n\times {\Sigma }_{i=1}^N{m}_i}$及$P_{k|k-1}^{-1}\in R^{n\times n},$可以得到是一个的对称矩阵，继而也是一个对称矩阵，且由实对称矩阵的性质知：存在正交矩阵和对角矩阵使得所以${\tilde{R}}_k^{-1}={\tilde{U}}_k{\tilde{\Sigma }}_k^{-1}{\tilde{U}}_k^T,$其中${\tilde{\Sigma }}_k=diag\left\{{\tilde{L}}_{1,k},{\tilde{\lambda }}_{2,k},...,{\tilde{\lambda }}_{{\Sigma }_{i=1}^N{m}_i,k}\right\},$是的第l个特征值，$(l=1,2,...,{\Sigma }_{i=1}^N{m}_i);$记${\tilde{\Sigma }}_{i,k}=diag\{{\tilde{\lambda }}_{{\Sigma }_{j=1}^{i-1}{m}_j+1,k},{\tilde{\lambda }}_{{\Sigma }_{j=1}^{i-1}{m}_j+2,k},...,{\tilde{\lambda }}_{{\Sigma }_{j=1}^{i-1}{m}_j+m_i,k}\},i=1,2,...,N$且${\tilde{\Sigma }}_{i,k}^{-1}=diag\{1/{\tilde{\lambda }}_{{\Sigma }_{j=1}^{i-1}{m}_j+1,k},1/{\tilde{\lambda }}_{{\Sigma }_{j=1}^{i-1}{m}_j+2,k},...,1/{\tilde{\lambda }}_{{\Sigma }_{j=1}^{i-1}{m}_j+m_i,k}\}$有：${\tilde{\Sigma }}_k=diag\{{\tilde{\Sigma }}_{1,k},{\tilde{\Sigma }}_{2,k},...,{\tilde{\Sigma }}_{N,k}\},{\tilde{\Sigma }}_k^{-1}=diag\{{\tilde{\Sigma }}_{1,k}^{-1},{\tilde{\Sigma }}_{2,k}^{-1},...,{\tilde{\Sigma }}_{N,k}^{-1}\}$根据噪声相关的扩展卡尔曼信息滤波器算法：$\{\begin{array}{c}{Y}_{k|k}=Y_{k|k-1}+I_k=Y_{k|k-1}+Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{R}}_k^{-1}{P}_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T\\ {\hat{y}}_{k|k}={\hat{y}}_{k|k-1}+i_k={\hat{y}}_{k|k-1}+Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{R}}_k^{-1}\left({\upsilon }_k+P_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T{\hat{x}}_{k|k-1}\right)\end{array}---\left(23\right)$得到信息矩阵I_k是由一些式子的线性组合组成：$\begin{array}{c}{I}_k=Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{R}}_k^{-1}{P}_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T\\ =Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{U}}_k{\tilde{\Sigma }}_k^{-1}{\tilde{U}}_k^T{P}_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T\\ =\underset{i=1}{\overset{N}{\Sigma }}{\tilde{\Omega }}_{i,k}{\tilde{\Sigma }}_k^{-1}{\tilde{\Omega }}_{i,k}^T\end{array}---\left(24\right)$其中，${\tilde{\Omega }}_k=Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{U}}_k$信息状态增量i_k的分散式为：$\begin{array}{c}{i}_k=Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{R}}_k^{-1}\left({\upsilon }_k+P_{\tilde{x}\tilde{z},k|k-1}{Y}_{k|k-1}^T{\hat{x}}_{k|k-1}\right)\\ =Y_{k|k-1}{P}_{\tilde{x}\tilde{z},k|k-1}{\tilde{U}}_k{\tilde{\Sigma }}_k^{-1}{\tilde{U}}_k^{-1}\left({\upsilon }_k+P_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T{\hat{x}}_{k|k-1}\right)\\ =\underset{i=1}{\overset{N}{\Sigma }}{\tilde{\Omega }}_{i,k}{\tilde{\Sigma }}_k^{-1}{T}_{i,k}\end{array}---\left(25\right)$其中，$T_k={\tilde{U}}_k^T({\upsilon }_k+P_{\tilde{x}\tilde{z},k|k-1}^T{Y}_{k|k-1}^T{\hat{x}}_{k|k-1})$步骤2.4利用下式计算k时刻信息矩阵Y_k|k和更新的信息状态向量${\tilde{R}}_k=R_k-D_k^T{P}_{k|k-1}^{-1}{D}_k---\left(26\right)$$\left\{\begin{array}{c}{\hat{y}}_{k|k}={\hat{y}}_{k|k-1}+i_k={\hat{y}}_{k|k-1}+\underset{i=1}{\overset{N}{\Sigma }}{\tilde{\Omega }}_{i,k}{\tilde{\Sigma }}_{i,k}^{-1}{T}_{i,k}\\ Y_{k|k}=Y_{k|k-1}+I_k=Y_{k|k-1}+\underset{i=1}{\overset{N}{\Sigma }}{\tilde{\Omega }}_{i,k}{\tilde{\Sigma }}_{i,k}^{-1}{\tilde{\Omega }}_{i,k}^T\end{array}\right.---\left(27\right)$步骤2.5用下式获得k时刻状态估计和相应协方差矩阵P_k|k；$\{\begin{array}{c}{P}_{k|k}=Y_{k|k}^{-1}\\ {\hat{x}}_{k|k}=P_{k|k}^{-1}{\hat{y}}_{k|k}\end{array}---\left(28\right)$不断重复上面两个模块的内容，就可实现对目标状态的跟踪估计。