一、测试程序
% PDA-FA算法实现
% 何友《雷达数据处理及应用》 P116
% 二维空间匀速直线运动,状态向量为 X=[x,vx,y,vy]
% x1=x0+vxT
% y1=y0+vyT
% 仿真:
% 1、改变虚假量测数量 nc :公式求取、手动设置
% 2、改变量测噪声 R=[r 0; 0 r] ,即 r
% 3、改变虚假量测位置 q,偏离真实位置的程度
% 4、关联概率计算
clc;
clear;
close all;
%************************************************
% 参数设置
%************************************************
I=eye(4);
T = 1; % 采样间隔
simTime = 100 ; % 仿真步数
A=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1]; % 实际模型: CV
H=[1 0 0 0;0 0 1 0]; % 测量模型
Q=0; % 实际过程噪声
G = [T^2/2 0; T 0; 0 T^2/2; 0 T]; % 噪声加权矩阵
r=200;
R=[r 0; 0 r]; % 量测噪声
X0=[200;0;10000;-15]; % 初始状态
X(:,1)=X0;
Vk=[sqrt(r)*randn;sqrt(r)*randn];
Zk(:,1)=H*X(:,1)+Vk;
gama=16;
lamda=0.0004;
%************************************************
% 量测生成
%************************************************
for i=2:1:simTime
X(:,i)=A*X(:,i-1); % 真实状态
Vk=[sqrt(r)*randn;sqrt(r)*randn];
Zk(:,i)=H*X(:,i)+Vk; % 生成量测值
end
%************************************************
% PDA 初始化
%************************************************
Xk_PDA=[200;0;10100;-16]; % 初始状态、与实际值略有差别
R11=r; R22=r; R12=0; R21=0;
Pkk_PDA=[R11 R11/T R12 R12/T;