多传感器模糊—概率交互作用的数据关联算法
加入技术交流群
(14)估计误差协方差矩阵为
-Vi(k)(Vi(k))T］(Ki(k))T (15)
Pi(k+1/k)=Φ(k)Pi(k/k)ΦT(k)+Q(k) (16)
(17)
Vij(k)=Zij(k)-Hi(k)Xi(k/k-1) (18)
Ki(k)=Pi(k/k-1)(Hi(k))T［Si(k)］-1 (19)
Si(k)=Hi(k)Pi(k/k-1)(Hi(k))T+Ri(k) (20)
其中βi0(k)是跟踪门设有一个测量来自于被跟踪目标的概率，其算式下一节给出.目标状态的多传感器估计为
(k/k)=∫X(k)P(X(k)/A,Z1,k-1,…,Zn,k-1)dX(k) (21)
为简化起见，我们仅考虑两个传感器的情形，对(k/k)的序列估计方法如下.
(1)预测 基于(k-1/k-1)和它的协方差P(k-1/k-1),分别应用以下各式计算预测的状态(k/k-1)和它的协方差P(k/k-1),以及预测的量测Z1(k)和相应的协方差S1(k).
(k/k-1)=Φ(k-1)(k-1/k-1) (22)
P(k/k-1)=Φ(k-1)P(k-1/k-1)ΦT(k-1)+Q(k-1) (23)
Z1(k)=H1(k)(k/k-1) (24)
S1(k)=H1(k)P(k/k-1)(H1(k))T+R1(k) (25)
(2)对第一个传感器确认量测 用Z1(k)，S1(k)按照下式确认量测
(V1j(k))T［S1(k)］-1V1j(k)＜γ1 (26)
其中V1j(k)=Z1j(k)-Z1(k).γ1为第一个传感器的跟踪门的门限值.
(3)用第一个传感器确认的量测进行状态估计
(27)
(28)
(4)对第二个传感器确认量测 基于(k/k)和P1(k/k)，按照下式确认量测.
Z2(k)=H2(k)1(k/k) (29)
S2(k)=H2(k)P1(k/k)(H2(k))T+R2(k) (30)
(V2j(k))T［S2(k)］-1V2j(k)＜γ2 (31)
其中V2j(k)=Z2j(k)-Z2(k).γ2为第二个传感器的跟踪门的门限值.
(5)用第二个传感器确认的量测计算状态估计和它的协方差，且作为多传感器的状态估计和它的协方差
评论