一、测试程序
PDAFA算法实现何友《雷达数据处理及应用》P116
二维空间匀速直线运动，状态向量为Xxvxyvyx1x0vxTy1y0vyT
仿真：1、改变虚假量测数量
c：公式求取、手动设置2、改变量测噪声Rr00r，即r3、改变虚假量测位置q，偏离真实位置的程度4、关联概率计算
clc
clear
closeall


参数设置

Ieye4
T1
采样间隔
simTime100
仿真步数
A1T000100001T0001实际模型：CV
H10000010测量模型
Q0
实际过程噪声
GT220T00T220T噪声加权矩阵
r200
Rr00r量测噪声
X020001000015初始状态
X1X0
Vksqrtrra
d
sqrtrra
d

Zk1HX1Vk
gama16
lamda00004


量测生成

fori21simTime
XiAXi1
真实状态
Vksqrtrra
d
sqrtrra
d

ZkiHXiVk
生成量测值
e
d


PDA初始化

Xk_PDA20001010016初始状态、与实际值略有差别
R11rR22rR120R210
Pkk_PDAR11R11TR12R12T
fR11T2R11T2R12T2R12T2
R21R21TR22R22T
R21T2R21T2R22T2R22T2初始协方差
XkkXk_PDA
PkkPkk_PDA
X_PreAXkk
P_PreAPkkAGQG
PR
fori11simTime


产生杂波

量测确认区域面积
SkHP_PreHP
AvpigamasqrtdetSk
准备生成杂波数目

cfloor10Avlamda1设置杂波数量
qsqrtAv2qsqrt10Av2
aX1iq
bX1iq
cX3iq
dX3iq
生成代表杂波的
c个虚假量测
xiabara
d1
c
yicdcra
d1
c
clearZ_Matrix
clearPZ_Matrix
forj1
c
Z_Matrixjxijyij
e
d
Z_Matrix
c1Zki
PZ_Matrixcat3
forj11
c
PZ_Matrixcat3PZ_Matrixq00q
e
d
PZ_Matrixcat3PZ_MatrixR


PDA关联

Z_PredictHX_Pre
PZ_PredictHP_PreH
Combi
e_ZCombi
e_RPDAZ_MatrixPZ_MatrixZ_PredictPZ_PredictPDA
Z_PDAiCombi
e_Z


卡尔曼滤波

PCombi
e_R
Xk_PDAPk_PDAKk_PDAKalma
XkkPkkCombi
e_ZAGQHP
XkkXk_PDA
fPkkPk_PDA
预测
X_PreAXkk
P_PreAPkkAGQG
出各个状态值
Ex_PDAiXkk1
Evx_PDAiXkk2
Ey_PDAiXkk3
Evy_PDAiXkk4
error1_PDAiEx_PDAiX1iPkk11
error2r