利用多个传感器对动态系统进行跟踪的方法及装置

摘要

本发明一种利用多个传感器对动态系统进行跟踪的方法及装置，所述方法包括：①建立动态系统运动模型；②将运动模型离散化，得到离散化运动模型；③利用多个传感器同时对车辆进行测量，建立离散化运动模型状态向量测量模型，得到测量方程；④对测量方程中测量噪声进行解相关，得到新的测量方程；⑤根据步骤②中离散化运动模型及步骤④中新的测量方程，利用卡尔曼滤波器对离散化运动模型中状态向量进行估计，并给出递归序贯式估计；⑥将步骤②中状态向量参数更新，同时将当前时刻状态向量估计值赋给前一时刻状态向量估计值，转向步骤③。本发明的积极效果是实现在传感器噪声相关情况下，降低对带宽和处理器的要求，同时提高定位精度。

说明书

技术领域

本发明涉及目标检测及跟踪技术领域，尤其涉及一种利用多个传感器对动态系统进行 跟踪的方法及装置。

背景技术

对动态系统，尤其是车辆进行跟踪，估计在目标跟踪领域有着非常广泛的应用。由于 单个传感器提供的测量信息有限，造成跟踪测量精度不高，定位不准。为了提高跟踪精度， 在实际应用当中多采用多个传感器同时对动态系统进行测量，然后采用集中式估计方法对 跟踪状态进行估计。

然而，多个传感器虽然可以提高跟踪定位的精度，但集中式估计算法的结构决定了多 个数据必须同时传递，否则就失去了实时性，而这一结构特点对传递信号的带宽有很高的 要求。另外，由于集中式的估计方法使得计算量成指数倍增加，从而对处理器计算速度的 要求也大大提高。这两点苛刻的要求，限制了对集中式估计方法的应用。

为了克服集中式估计方法的不足，人们在实际应用中通常采用分布式估计方法或序贯 式估计方法，这两种方法均在一定程度上降低了对带宽和处理器的要求，特别是序贯式估 计方法，只需要一个处理器依次按照测量值的到来顺序，对状态进行估计，从而大大降低 了对处理器的要求。然后，在复杂的工作环境下，传感器测量噪声之间通常是相关的，从 而传统的分布式估计方法和序贯式估计方法将不能应用。

针对噪声相关的情况，已有人对分布式估计方法进行了改进，但是，已有的方法或者 是计算量复杂或者是舍弃了互相关噪声的一些信息，从而造成估计精度下降，无法等价于 集中式估计方法。

基于集中式估计方法的缺点以及噪声相关时分布式估计存在的不足，对序贯式估计方 法进行改进使其能够在噪声相关的情况下进行应用成了一个很好的选择，目前针对噪声相 关时序贯式估计方法的改进研究还较少。

发明内容

本发明的目的在于解决上述问题，提供一种利用多个传感器对动态系统进行跟踪的方 法，实现在传感器噪声相关的情况下，降低对带宽和处理器的要求，同时提高定位精度。 本发明的再一目的是，按所述利用多个传感器对动态系统进行跟踪的方法提供一种利用多 个传感器对动态系统进行跟踪装置，实现在传感器噪声相关的情况下，降低对带宽和处理 器的要求，同时提高定位精度。

为实现上述目的，本发明采取了以下技术方案。

一种利用多个传感器对动态系统进行跟踪的方法，其特征在于，包括如下步骤：

（1）建立动态系统的运动模型；

（2）将所述运动模型离散化，得到离散化运动模型，所述离散化运动模型包括状态向量 及所述状态向量的参数；

（3）利用多个传感器同时对车辆进行测量，建立所述离散化运动模型的状态向量的测量 模型，得到测量方程；

（4）对测量方程中的测量噪声进行解相关，得到新的测量方程；

（5）根据步骤（2）中的离散化运动模型及步骤（4）中的新的测量方程，利用卡尔曼滤 波器对离散化运动模型中的状态向量进行估计，并给出递归的序贯式估计；

（6）将步骤（2）中状态向量的参数更新，同时将当前时刻状态向量的估计值赋给前一 时刻状态向量的估计值，转向步骤（3）。

进一步，所述步骤（1）的运动模型为：

$\stackrel{·}{x}\left(t\right)=A\left(t\right)x\left(t\right)+w\left(t\right)$

x(t)＝[x₁(t)  x₂(t)  x₃(t)]^T

其中，t为时间值，x(t)为动态模型的状态向量，A(t)为系统状态转移矩阵，w(t)为系统 噪声；x₁(t)表示位置；x₂(t)表示速度；x₃(t)表示加速度。

进一步，所述步骤（2）的所述离散化运动模型中状态向量的表达式为：

x(k)＝Φ(k)x(k)+Γ(k)w(k)

其中，k为采样时刻值，为正整数；x(k)为k时刻的动态模型的状态向量，Φ(k)为离散 系统的状态转移矩阵，Γ(k)为噪声转移矩阵，w(k)为离散化后的系统噪声，零均值，w(k)的 协方差为。

进一步，所述步骤（3）的测量方程为：

z_i(k)＝H_i(k)x(k)+v_i(k)  i＝1，2，…，N

测量噪声v_i(k)i＝1，2，…，N是互相关的，统计特性为：

进一步，所述步骤（4）中对测量方程中的测量噪声进行解相关的方法是根据各个传感器 测量噪声的相关性矩阵S(k)，采用逐个解相关的方法处理，具体步骤包括：依次将前i个传 感器的测量值叠加，得到：

$Z_i\left(k\right)={[z_1^T\left(k\right),z_2^T\left(k\right)···,z_i^T\left(k\right)]}^T,i=\mathrm{1,2},···,N$

相应的测量噪声和测量矩阵为：

$V_i\left(k\right)={[v_1^T\left(k\right),v_2^T\left(k\right)···,v_i^T\left(k\right)]}^T,i=\mathrm{1,2},···,N$

${\Psi }_i\left(k\right)={[H_1^T\left(k\right),H_2^T\left(k\right),···,H_i^T\left(k\right)]}^T,i=\mathrm{1,2},···,N$

v_i(k)与V_i(k),i＝1，2，…，N之间的互协方差为：

${\bar{S}}_i\left(k\right)=E\left\{v_i\left(k\right)V_{i-1}^T\left(k\right)\right\}$

$=[S_{i1}\left(k\right),S_{i2}\left(k\right),···,S_{\mathrm{ii}-1}\left(k\right)]$

V_i(k)的协方差为：

定义：

$G_i\left(k\right)={\bar{S}}_i\left(k\right){\Lambda }_{i-1}^{-1}\left(k\right)$

根据步骤（3）中的测量方程及上述公式，Z_i(k)的测量方程为：

Z_i(k)＝Ψ_i(k)x(k)+V_i(k)  i＝1，2,…，N

或

$z_i\left(k\right)=\left(\begin{array}{cc}{H}_1\left(k\right)x\left(k\right)+v_1\left(k\right)& i=1\\ H_i\left(k\right)x\left(k\right)+v_i\left(k\right)-G_i\left(k\right)V_{i-1}\left(k\right)+G_i\left(k\right)V_{i-1}\left(k\right)& i=\mathrm{2,3},···,N\end{array}\right)$

则z_i(k)，i＝2,3,…,N可以重新描述为：

$z_i\left(k\right)=\left(\begin{array}{cc}{H}_1\left(k\right)x\left(k\right)+v_1\left(k\right)& i=1\\ H_i\left(k\right)x\left(k\right)+v_i\left(k\right)-G_i\left(k\right)V_{i-1}\left(k\right)+G_i\left(k\right)[Z_{i-1}\left(k\right)-{\Psi }_{i-1}\left(k\right)x\left(k\right)]& i=\mathrm{2,3},···,N\end{array}\right)$

基于上述变换，新的测量方程为：

${\bar{z}}_i\left(k\right)={\bar{H}}_i\left(k\right)x\left(k\right)+{\bar{v}}_i\left(k\right)$

其中：

${\bar{z}}_i\left(k\right)=\left(\begin{array}{cc}{z}_1\left(k\right),& i=1\\ z_i\left(k\right)-G_i\left(k\right)Z_{i-1}\left(k\right),& i=\mathrm{2,3},···,N\end{array}\right)$

${\bar{v}}_i\left(k\right)=\left(\begin{array}{cc}{v}_1\left(k\right),& i=1\\ v_i\left(k\right)-G_i\left(k\right)V_{i-1}\left(k\right),& i=\mathrm{2,3},···,N\end{array}\right)$

${\bar{H}}_i\left(k\right)=\left(\begin{array}{cc}{H}_1\left(k\right),& i=1\\ H_i\left(k\right)-G_i\left(k\right){\Psi }_{i-1}\left(k\right),& i=\mathrm{2,3},···,N\end{array}\right)$

所述新测量方程的噪声协方差为：

${\bar{R}}_i\left(k\right)=E\left\{{\bar{v}}_i\left(k\right){\bar{v}}_i^T\left(k\right)\right\}.$

进一步，所述步骤（5）进一步包括：依照传感器测量数据到来的顺序进行估计，过程 如下：

第一个传感器测量数据到来时：

${\hat{x}}_1\left(k\right|k)=E\left\{x\left(k\right)\right|Z_N^{k-1},{\bar{z}}_1\left(k\right)\}$

$=\hat{x}\left(k\right|k-1)+K_1^{\left(\alpha \right)}\left(k\right)[{\bar{z}}_1\left(k\right)-{\bar{H}}_1\left(k\right)\hat{x}\left(k\right|k-1)]$

$K_1\left(k\right)=P\left(k\right|k-1){\bar{H}}_1^T\left(k\right){[{\bar{H}}_1\left(k\right)P\left(k\right|k-1){\bar{H}}_1^T\left(k\right)+{\bar{R}}_1\left(k\right)]}^{-1}$

$P_1\left(k\right|k)=[I-K_1\left(k\right){\bar{H}}_1\left(k\right)]P\left(k\right|k-1)$

第i，i＝2，3,…,N个传感器的测量值到来时：

${\hat{x}}_i\left(k\right|k)=E\left\{x\left(k\right)\right|Z_N^{k-1},{\bar{z}}_1\left(k\right),{\bar{z}}_2\left(k\right),···,{\bar{z}}_i\left(k\right)\}$

$={\hat{x}}_{i-1}\left(k\right|k)+K_i^{\left(\alpha \right)}\left(k\right)[{\bar{z}}_i\left(k\right)-{\bar{H}}_i\left(k\right){\hat{x}}_{i-1}\left(k\right|k)]$

$K_i\left(k\right)=P_{i-1}\left(k\right|k){\bar{H}}_i^T\left(k\right){[{\bar{H}}_i\left(k\right)P_{i-1}\left(k\right|k){\bar{H}}_i^T\left(k\right)+{\bar{R}}_i\left(k\right)]}^{-1}$

$P_i\left(k\right|k)=[I-K_i\left(k\right){\bar{H}}_i\left(k\right)]P_{i-1}\left(k\right|k)$

其中，表示动态系统状态向量x(k)依据前i个传感器的测量值得到的 估计值；P_i(k|k)表示估计误差协方差；K_i(k|k)表示滤波增益；

当k时刻所有传感器测量值都得到并应用于状态估计后，最终得到k时刻动态系统状 态向量x(k)的最优估计结果：

$\hat{x}\left(k\right|k):={\hat{x}}_N\left(k\right|k)$

P(k|k):＝P_N(k|k)。

进一步，所述步骤（6）进一步包括：对步骤（2）中的状态向量参数A(k)进行更 新，同时将k时刻状态向量的估计值赋给k-1时刻状态向量的估计值，转向步骤（3）。

为实现上述第二目的，本发明采取了以下技术方案。

一种利用多个传感器对动态系统进行跟踪的装置，其特征在于，包括建立单元、离散 化单元、构造单元、处理单元、估计单元及更新单元：

所述建立单元，用于建立动态系统的运动模型；

所述离散化单元，用于将所述运动模型离散化，得到离散化运动模型，所述离散化运 动模型包括状态向量及所述状态向量的参数；

所述构造单元，用于利用多个传感器同时对车辆进行测量，建立所述离散化运动模型 的状态向量的测量模型，得到测量方程；

所述处理单元，用于对测量方程中的测量噪声进行解相关，得到新的测量方程;

所述估计单元，用于根据离散化运动模型及新的测量方程，利用卡尔曼滤波器对离散 化运动模型中的状态向量进行估计，并给出递归的序贯式估计；

所述更新单元，用于将离散化单元中的状态向量的参数更新，同时将当前时刻状态向 量的估计值赋给前一时刻状态向量的估计值，并转向构造单元。

本发明利用多个传感器对动态系统进行跟踪的方法及装置的积极效果是：将传统的噪 声不相关时的序贯式估计方法应用到噪声相关时的情况，基于原有的测量方程构造出全新 的测量方程，新的测量方程与原有的测量方程提供的信息等效，从而，在传感器噪声相关 的情况下，降低对带宽和处理器的要求，同时提高定位精度。

附图说明

图1是本发明一种利用多个传感器对动态系统进行跟踪的方法的流程框图。

图2是本发明一种利用多个传感器对动态系统进行跟踪装置的结构连接框图。

图3是本发明一种利用多个传感器对动态系统进行跟踪的方法应用实施例示意图。

图4是运动系统实际位置、估计位置与时间的关系图。

图5是运动系统实际速度、估计速度与时间的关系图。

图6是运动系统实际加速度、估计加速度与时间的关系图。

具体实施方式

下面结合附图对本发明提供的一种利用多个传感器对动态系统进行跟踪的方法及装置 的具体实施方式做详细说明。

参见图1，一种利用多个传感器对动态系统进行跟踪的方法，包括如下步骤：

S11：建立动态系统的运动模型

当车辆沿直线运动的时候，车辆的整体状态方程可表示为：

$\stackrel{·}{x}\left(t\right)=A\left(t\right)x\left(t\right)+w\left(t\right)---\left(1\right)$

x(t)＝[x₁(t)  x₂(t)  x₃(t)]^T    (2)

其中t是时间值，x(t)为动态模型的状态向量，A(t)为系统状态转移矩阵，w(t)为系统 噪声；x₁(t)表示位置；x₂(t)表示速度；x₃(t)表示加速度。

S12：将所述运动模型离散化，得到离散化运动模型，所述离散化运动模型包括状态向量 及所述状态向量的参数

对该模型进行离散化处理，可得离散的状态表达式：

x(k)＝Φ(k)x(k)+Γ(k)w(k)    (3)

其中k为采样时刻值，为正整数；x(k)为k时刻的动态模型的状态向量，Φ(k)为离散 系统的状态转移矩阵，Γ(k)为噪声转移矩阵，w(k)为离散化后的系统噪声，零均值，w(k) 的协方差为Q(k)。

S13：利用多个传感器同时对车辆进行测量，建立所述离散化运动模型的状态向量的测量 模型，得到测量方程

利用传感器的测量值，建立步骤一中所描述的动态模型中状态x(k)的测量模型：

利用N个传感器同事对车辆进行测量，测量方程为：

z_i(k)＝H_i(k)x(k)+v_i(k)  i＝1，2，…，N    （4）

测量噪声v_i(k)，i＝1，2，…，N是互相关的，统计特性为：

由于测量噪声之间是相关的，因此无法直接利用传统的序贯式滤波对车辆的状态进行估 计。

S14：对测量方程中的测量噪声进行解相关，得到新的测量方程

由于步骤S13中k时刻的测量噪声是相关的，因此传统的序贯式卡尔曼滤波无法应用； 根据各个传感器测量噪声的相关性矩阵S(k)，采用逐个解相关的方法处理，具体为：

依次将前i个传感器的测量值叠加起来，有：

$Z_i\left(k\right)={[z_1^T\left(k\right),z_2^T\left(k\right)···,z_i^T\left(k\right)]}^T,i=\mathrm{1,2},···,N---\left(6\right)$

相应的测量噪声和测量矩阵为：

$V_i\left(k\right)={[v_1^T\left(k\right),v_2^T\left(k\right)···,v_i^T\left(k\right)]}^T,i=\mathrm{1,2},···,N---\left(7\right)$

${\Psi }_i\left(k\right)={[H_1^T\left(k\right),H_2^T\left(k\right),···,H_i^T\left(k\right)]}^T,i=\mathrm{1,2},···,N---\left(8\right)$

根据上式中的定义，v_i(k)与V_i(k),i＝1，2，…，N之间的互协方差为：

${\bar{S}}_i\left(k\right)=E\left\{v_i\left(k\right)V_{i-1}^T\left(k\right)\right\}$$\left(9\right)$

$=[S_{i1}\left(k\right),S_{i2}\left(k\right),···,S_{\mathrm{ii}-1}\left(k\right)]$

V_i(k)的协方差为：

定义：

$G_i\left(k\right)={\bar{S}}_i\left(k\right){\Lambda }_{i-1}^{-1}\left(k\right)---\left(11\right)$

利用公式（4）以及前面几个新定义的公式，Z_i(k)的测量方程可以描述为：

Z_i(k)＝Ψ_i(k)x(k)+V_i(k),i＝1，2,…，N    (12)

另一方面，式（4）所描述的测量方程也可以重新描述为：

$z_i\left(k\right)=\left(\begin{array}{cc}{H}_1\left(k\right)x\left(k\right)+v_1\left(k\right)& i=1\\ H_i\left(k\right)x\left(k\right)+v_i\left(k\right)-G_i\left(k\right)V_{i-1}\left(k\right)+G_i\left(k\right)V_{i-1}\left(k\right)& i=\mathrm{2,3},···,N\end{array}\right)---\left(13\right)$

结合(13)，z_i(k)，i＝2,3,…,N可以重新描述为

$z_i\left(k\right)=\left(\begin{array}{cc}{H}_1\left(k\right)x\left(k\right)+v_1\left(k\right)& i=1\\ H_i\left(k\right)x\left(k\right)+v_i\left(k\right)-G_i\left(k\right)V_{i-1}\left(k\right)+G_i\left(k\right)[Z_{i-1}\left(k\right)-{\Psi }_{i-1}\left(k\right)x\left(k\right)]& i=\mathrm{2,3},···,N\end{array}\right)---\left(14\right)$

基于上述变换，测量方程(4)可以等价描述为：

${\bar{z}}_i\left(k\right)={\bar{H}}_i\left(k\right)x\left(k\right)+{\bar{v}}_i\left(k\right),i=\mathrm{1,2},···,N---\left(15\right)$

其中：

${\bar{z}}_i\left(k\right)=\left(\begin{array}{cc}{z}_1\left(k\right),& i=1\\ z_i\left(k\right)-G_i\left(k\right)Z_{i-1}\left(k\right),& i=\mathrm{2,3},···,N\end{array}\right)---\left(16\right)$

${\bar{v}}_i\left(k\right)=\left(\begin{array}{cc}{v}_1\left(k\right),& i=1\\ v_i\left(k\right)-G_i\left(k\right)V_{i-1}\left(k\right),& i=\mathrm{2,3},···,N\end{array}\right)---\left(17\right)$

${\bar{H}}_i\left(k\right)=\left(\begin{array}{cc}{H}_1\left(k\right),& i=1\\ H_i\left(k\right)-G_i\left(k\right){\Psi }_{i-1}\left(k\right),& i=\mathrm{2,3},···,N\end{array}\right)---\left(18\right)$

新测量方程的噪声协方差为：

${\bar{R}}_i\left(k\right)=E\left\{{\bar{v}}_i\left(k\right){\bar{v}}_i^T\left(k\right)\right\}---\left(19\right)$

可以验证新的不同测量方程的噪声满足说明他们是互不相关的，解 相关步骤完成。

S15：根据步骤S13中的离散化运动模型及步骤S14中的新的测量方程，利用卡尔曼滤 波器对离散化运动模型中的状态向量进行估计，并给出递归的序贯式估计

根据步骤S13中的离散化运动模型及步骤S14中的新的测量方程i＝2,3,…，N结 合构造新测量方程中的噪声方差依次利用卡尔曼滤波器对系统误差模型的状态向量 x(k)进行估计，具体为：

依照传感器测量数据到来的顺序进行估计，计算过程如公式（20）-（25）所示：

第1个传感器到来时：

${\hat{x}}_1\left(k\right|k)=E\left\{x\left(k\right)\right|Z_N^{k-1},{\bar{z}}_1\left(k\right)\}$$\left(20\right)$

$=\hat{x}\left(k\right|k-1)+K_1^{\left(\alpha \right)}\left(k\right)[{\bar{z}}_1\left(k\right)-{\bar{H}}_1\left(k\right)\hat{x}\left(k\right|k-1)]$

$K_1\left(k\right)=P\left(k\right|k-1){\bar{H}}_1^T\left(k\right){[{\bar{H}}_1\left(k\right)P\left(k\right|k-1){\bar{H}}_1^T\left(k\right)+{\bar{R}}_1\left(k\right)]}^{-1}---\left(21\right)$

$P_1\left(k\right|k)=[I-K_1\left(k\right){\bar{H}}_1\left(k\right)]P\left(k\right|k-1)---\left(22\right)$

第i，i＝2,3,…,N个传感器的测量值到来时：

${\hat{x}}_i\left(k\right|k)=E\left\{x\left(k\right)\right|Z_N^{k-1},{\bar{z}}_1\left(k\right),{\bar{z}}_2\left(k\right),···,{\bar{z}}_i\left(k\right)\}$$\left(23\right)$

$={\hat{x}}_{i-1}\left(k\right|k)+K_i^{\left(\alpha \right)}\left(k\right)[{\bar{z}}_i\left(k\right)-{\bar{H}}_i\left(k\right){\hat{x}}_{i-1}\left(k\right|k)]$

$K_i\left(k\right)=P_{i-1}\left(k\right|k){\bar{H}}_i^T\left(k\right){[{\bar{H}}_i\left(k\right)P_{i-1}\left(k\right|k){\bar{H}}_i^T\left(k\right)+{\bar{R}}_i\left(k\right)]}^{-1}---\left(24\right)$

$P_i\left(k\right|k)=[I-K_i\left(k\right){\bar{H}}_i\left(k\right)]P_{i-1}\left(k\right|k),i=\mathrm{2,3},···,N---\left(25\right)$

其中，表示动态系统状态向量x(k)依据前i个传感器的测量值得到的 估计值；P_i(k|k)表示估计误差协方差；K_i(k|k)表示滤波增益；

当k时刻所有传感器测量值都得到并应用于状态估计后，最终得到k时刻动态系统状 态向量x(k)的最优估计结果：

$\hat{x}\left(k\right|k):={\hat{x}}_N\left(k\right|k)$

P(k|k):＝P_N(k|k)。

S16：将步骤S12中状态向量的参数更新，同时将当前时刻状态向量的估计值赋给前一 时刻状态向量的估计值，转向步骤S13。

对步骤S12中的动态模型中的参数A(k)进行更新，同时将k时刻状态的估计值赋给 k-1时刻状态的估计值，然后转向步骤S13。

本发明一种利用多个传感器对动态系统进行跟踪的方法将传统的噪声不相关时的序贯 式估计方法应用到噪声相关时的情况，基于原有的测量方程构造出全新的测量方程，新的 测量方程与原有的测量方程提供的信息等效，从而，在传感器噪声相关的情况下，降低对 带宽和处理器的要求，同时提高定位精度。

参见图2，一种利用多个传感器对动态系统进行跟踪的方法装置，包括建立单元21、 离散化单元22、构造单元23、处理单元24、估计单元25及更新单元26。

所述建立单元21，用于建立动态系统的运动模型。所述离散化单元22，用于将所述运 动模型离散化，得到离散化运动模型，所述离散化运动模型包括状态向量及所述状态向量 的参数。所述构造单元23，用于利用多个传感器同时对车辆进行测量，建立所述离散化运 动模型的状态向量的测量模型，得到测量方程。所述处理单元24于对测量方程中的测量噪 声进行解相关，得到新的测量方程。所述估计单元25于根据离散化运动模型及新的测量方 程，利用卡尔曼滤波器对离散化运动模型中的状态向量进行估计，并给出递归的序贯式估 计。所述更新单元26，用于将离散化单元中的状态向量的参数更新，同时将当前时刻状态 向量的估计值赋给前一时刻状态向量的估计值，并转向构造单元23。

以下结合图3给出本发明的应用实施例，对本发明做进一步的描述。

1.选定仿真时利用的参数

在利用MATLAB软件对本方法进行实现的时候，选取参数为：

$A=\left(\begin{array}{ccc}1& T& 0.5T^2\\ 0& 1& T\\ 0.& 0& 1\end{array}\right),$$Q=\left(\begin{array}{ccc}0.9& 0.2& 0\\ 0.2& 1& 0.1\\ 0& 0.1& 0.5\end{array}\right),$

采用三个传感器对动态系统进行测量，具体参数为：

H₁＝[1  0.4  1]，H₂＝[0.5  0  2]，$H_3=\left(\begin{array}{ccc}1& 1& 0\\ 0& 1& 1\end{array}\right),$T＝0.1，监测时间L为100s。传感器噪 声统计特性为：R₁(k)＝3,R₂(k)＝1,$R_3\left(k\right)=\left(\begin{array}{cc}2& 0\\ 0& 0.8\end{array}\right),$噪声之间相关为：S₁₂(k)＝0.5, s₁₃(k)＝[0.2  0.1],S₂₃(k)＝[0.2  0.6]，初始值x₀＝[0  0  1]^T,P₀＝Diag{0  0  0.05}。

2.对系统状态进行一步预测

在k时刻，若系统在k-1时刻的状态x(k-1)的估计值和误差协方差 P(k-1|k-1)已知（初始值取P(0|0)＝P₀），对k时刻的状态值进行预测，具体为： $\hat{x}\left(k\right|k-1)=\Phi (k-1)\hat{x}(k-1|k-1).$

相应的预测误差协方差为：

P(k|k-1)＝Φ(k-1)P(k-1|k-1)Φ^T(k-1)+Q(k-1)。

3.对测量噪声解相关

利用1中给出的测量噪声的相关信息以及测量方程参数，进行解相关。

依次将前i个传感器的测量值叠加起来，有：

$Z_i\left(k\right)={[z_1^T\left(k\right),z_2^T\left(k\right)···,z_i^T\left(k\right)]}^T,i=\mathrm{1,2},···,N$

相应的测量噪声和测量矩阵为：

$V_i\left(k\right)={[v_1^T\left(k\right),v_2^T\left(k\right)···,v_i^T\left(k\right)]}^T,i=\mathrm{1,2},···,N$

${\Psi }_i\left(k\right)={[H_1^T\left(k\right),H_2^T\left(k\right),···,H_i^T\left(k\right)]}^T,i=\mathrm{1,2},···,N$

根据上式中的定义，v_i(k)与V_i(k),i＝1，2，…，N之间的互协方差为：

${\bar{S}}_i\left(k\right)=E\left\{v_i\left(k\right)V_{i-1}^T\left(k\right)\right\}$

$=[S_{i1}\left(k\right),S_{i2}\left(k\right),···,S_{\mathrm{ii}-1}\left(k\right)]$

V_i(k)的协方差为：

定义：$G_i\left(k\right)={\bar{S}}_i\left(k\right){\Lambda }_{i-1}^{-1}\left(k\right)$

利用上述公式，Z_i(k)的测量方程可以描述为

Z_i(k)＝Ψ_i(k)x(k)+V_i(k),i＝1，2,…，N

另一方面，测量方程也可以重新描述为：

$z_i\left(k\right)=\left(\begin{array}{cc}{H}_1\left(k\right)x\left(k\right)+v_1\left(k\right)& i=1\\ H_i\left(k\right)x\left(k\right)+v_i\left(k\right)-G_i\left(k\right)V_{i-1}\left(k\right)+G_i\left(k\right)V_{i-1}\left(k\right)& i=\mathrm{2,3},···,N\end{array}\right)$

结合上述两个公式，z_i(k)，i＝2,3,…,N可以重新描述为

$z_i\left(k\right)=\left(\begin{array}{cc}{H}_1\left(k\right)x\left(k\right)+v_1\left(k\right)& i=1\\ H_i\left(k\right)x\left(k\right)+v_i\left(k\right)-G_i\left(k\right)V_{i-1}\left(k\right)+G_i\left(k\right)[Z_{i-1}\left(k\right)-{\Psi }_{i-1}\left(k\right)x\left(k\right)]& i=\mathrm{2,3},···,N\end{array}\right)$

基于上述变换，测量方程(4)可以等价描述为：

${\bar{z}}_i\left(k\right)={\bar{H}}_i\left(k\right)x\left(k\right)+{\bar{v}}_i\left(k\right),i=\mathrm{1,2},···,N$

其中：

${\bar{z}}_i\left(k\right)=\left(\begin{array}{cc}{z}_1\left(k\right),& i=1\\ z_i\left(k\right)-G_i\left(k\right)Z_{i-1}\left(k\right),& i=\mathrm{2,3},···,N\end{array}\right)$

${\bar{v}}_i\left(k\right)=\left(\begin{array}{cc}{v}_1\left(k\right),& i=1\\ v_i\left(k\right)-G_i\left(k\right)V_{i-1}\left(k\right),& i=\mathrm{2,3},···,N\end{array}\right)$

${\bar{H}}_i\left(k\right)=\left(\begin{array}{cc}{H}_1\left(k\right),& i=1\\ H_i\left(k\right)-G_i\left(k\right){\Psi }_{i-1}\left(k\right),& i=\mathrm{2,3},···,N\end{array}\right)$

新测量方程的噪声协方差为：

${\bar{R}}_i\left(k\right)=E\left\{{\bar{v}}_i\left(k\right){\bar{v}}_i^T\left(k\right)\right\}$

可以验证新的不同测量方程的噪声满足说明他们是互不相关的，解 相关步骤完成。

4.利用3中得到的新的测量方程，对2中的预测值进行逐步更新，具体为：

依照传感器测量数据到来的顺序进行估计，计算过程如上述公式所示，当k时刻所有 传感器测量值都得到并应用于状态估计后，最终得到k时刻动态系统状态向量x(k)的最优 估计结果：

$\hat{x}\left(k\right|k):={\hat{x}}_N\left(k\right|k)$

P(k|k):＝P_N(k|k)。

5.根据4中的计算结果，回到2中，同时将的值赋给将P(k|k)的 值赋给P(k-1|k-1)。

本实施例中给出了对运动系统位置、速度及加速度的估计，参见图4、图5及图6所示， 从图中可以看出，本发明所提方法对车辆的位置和速度有很好的估计。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员， 在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本 发明的保护范围。