非机动模式（cv模型）：
Xk?1??Xk?GW?k?
&X???k??x?k?x?1?0????0??0T（12）
T式中：
y?k?&y?k???
（13）
00?100??01T?
?001?（14）
?T220???T0?G??2?0T2? ??0T???us?k??W?k????
uk???y?（15）
（16）
2?us其中
us?k?、
uy?k?分别为相互独立的零均值方差和
?u2y的高斯白噪声。
（17） （18） （19）
2量测模型为：
Z?k??C?k?X?k??V?k??1000?C????0010?
?vx?k??V?k????vk??y??
其中
vx?k?、
vy?k?是相互独立的高斯白噪声，均值为0，方差为?。
机动模型（CA模型）：
Xkm?1??mXkm?GmWm?k?&Xm???k??x?k?xy?k?
T（20）
式中：上标m表示机动
&&&yxy?k?&?k?&?k???
（21）
?1??0?0m????0?0???0T00T22T001010001T0010000000??0?T22??T? 0??1??（22）
0??T24??0T2??2?0T4?Gm???T20??
?10???1???0??ux?k??W?k????uk???y?
（23）
（24）
2?u其中：、分别为相互独立的零均值方差x与y之和的高斯白噪声。 量测模型与非机动模型的量测模型一样，但模型中的矩阵C变成：
?100000?C?? ?（25）
?001000?
4.1.5 VD滤波算法
VD滤波算法是在二维卡尔曼滤波基础上采用两种模型，即非机动模型和机动模型，无机动时滤波器处于正常模式，用一机动检测器监视机动，一旦检测到机动，模型中立即增加一个状态变量，用机动模型跟踪直到下一次判决而退回至正常的非机动模型。
机动检测：
滤波器开始工作于正常模式（非机动模式），其输出的新息序列为v（k），令
??k?????k?1????k?
（26） （27）
ux?k?uy?k??u2??k??vT?k?S?1?k?v?k?
Skvkvk?kn其中：??是??的协方差矩阵，由于??是零均值高斯随机变量，所以??服从xnx?1limE????22k??k???1?a????1?a，（量测维数）为自由度的分布，也服从分布，且取
??Th作为检测机动的有效窗口长度，机动检测的方法为：如果k，则认为目标在k???1开始有一恒定的加速度加入，这是目标模型应由低阶模型转向高阶模型。由高阶机动模型退回低阶非机动模型的检测方法是检测加速度估计值是否有统计显著性意义。
令
?a?k??
j???t???j?ak
?1（28）
（29）
m?T?k,k????k,k??a?j??aPa??k/k???a其中：是加速度分量的估计值，是协方差矩阵的对应块，如果
?a?k??Ta，则加速度估计无显著意义，滤波器退出机动模型。
滤波初始化：
??k,k?aPam?k/k?
当在第k次检测到机动时，滤波器假定在k???1时刻开始有一恒定加速度，在窗内的状态估计应修正如下:
首先，加速度在k??的估计为
m?xa?k??/k????
m?ya?k??/k????2??k??/k???1???Zk????Zx2?x??T2??k??/k???1???Zk????Zy2?y??T （30） （31）
?Zx是对测量的预测值，
??k??/k???1??x??k??/k???1?Zx
（32） （33）
??k??/k???1??y??k??/k???1?Zy
在k??的位置估计为：
?m?k??/k???1??Zx?k???x
（34） （35）
?m?k??/k???1??Zy?k???y
在k??的速度估计为：
Vxm?k??/k????Vx?k???1/k???1??Tax?k???1/k???1?
（36） （37）
Vym?k??/k????Vy?k???1/k???1??Tay?k???1/k???1?
mP11?k??/k????R112 mPR1112?k??/k????T mP15?k??/k????协方差矩阵修正为：
（38）
4m2P55R?Pk???1/k???1?2TPk???1/k???1?TP22?k???1/k???1???k??/k????2?????111112?T?
mP22?k??/k????2R11T2
444R?Pk???1/k???1?Pk???1/k???1?P????11112212?k???1/k???1?T2T2T 4426R?Pk???1/k???1?Pk???1/k???1?P????11112212?k???1/k???1?T3T3TT2
mP25?k??/k????
4.1.6 误差分析
由于采用卡尔曼滤波是在直角坐标系下进行的，利用坐标转换将极坐标系下的测量值转换至直角坐标系下，再对所得转换后的测量误差的均值和方差进行相应的去偏之后，才能有效的利用卡尔曼滤波器进行滤波。
题目中雷达均位于各自极坐标系下原点处，目标的距离、方位角和俯仰角的测量值为
rm，
?m，?m，其与真实位置r，?，?的关系为
rm?r??r?m?????
（39）
?m?????
2???，??均为互不相关的零均值高斯白噪声，上式中，测量误差?r ，其方差分别为r，2????2 ， 。
对上式进行坐标变换，得直角坐标系下测量方程：
xm?rmsin?mcos?m
ym?rmcos?mcos?m
（40）
zm?rmsin?m
式（2）中，设
xm，
ym ，
zm
为直角坐标系下测量值x， y，z ， 为其真实值，则转
换测量误差可表示为：
?x?xm?x?(r??r)sin(????)cos(????)?rsin?cos?
?y?ym?y?(r??r)cos(????)cos(????)?rcos?cos?
（41）
?z?zm?z?(r??r)sin(????)?rsin?
转换测量值的均值
?a
可表示为：
22??????22????????22?e)??rmsin?mcos?m(e??E??x|rm,?m,?m???2?2???????????2??2?a??E??y|rm,?m,?m????rmcos?mcos?m(e???e22)?2???? （42） ??E?z|r,?,???2mmm????????rmsin?m(e?e2)????由上式可知，直角坐标系下的转换测量值是有偏的。进行去偏处理，修正后的测量值可表示为：
?xdc??rmsin?mcos?m??dc??? ?y???rmcos?mcos?m???a
?zdc?? （43）?????rmsin?m
4.1.7 滤波结果及机动目标的航迹图
雷达1探测到的信息滤波处理后得到的机动目标航迹如图4.1.8所示：
图4.1.8 雷达1探测到的目标航迹
可以看出滤波后机动目标的运动轨迹包括一段直线，一段圆弧，和一段误差较大的直线。
雷达2探测到的信息滤波处理后得到的机动目标航迹如图4.1.9所示：
图4.1.9 雷达2探测到的目标航迹
可以看出滤波后机动目标的运动轨迹接近于一段椭圆和一段直线。
雷达3探测到的信息滤波处理后得到的机动目标航迹如图4.1.10所示：
(XXXX研究生数学建模竞赛)机动目标的跟踪与反跟踪
