先看结果
clc
clear all
close all
%% 雷达
radar0=[0,0,0,0.05*pi/180,0.1*pi/180]; %主 被动雷达
radar1=[10000,3000,0,0.05*pi/180,0.1*pi/180]; %副
radar2=[3000,10000,0,0.05*pi/180,0.1*pi/180]; %副
radar3=[5000,6000,10,0.05*pi/180,0.1*pi/180]; %副
Radar_fu=[radar1;
radar2;
radar3;];
%% 目标
T=1;%步长
F = [
1 , T , 0 , 0 ,0 , 0;
0 , 1 , 0 , 0 ,0 , 0;
0 , 0 , 1 , T ,0 , 0;
0 , 0 , 0 , 1 ,0 , 0;
0 , 0 , 0 , 0 ,1 , T;
0 , 0 , 0 , 0 ,0 , 1;
];
q1 = 0.5; %目标的运动噪声标准差
Q = [
q1 * T^3 / 3 , q1 * T^2 / 2 , 0 , 0 , 0 , 0 ;
q1 * T^2 / 2 , q1 * T ,