track_cv.m

来自「基于2D和红外的异类传感器的数据融合算法」· M 代码 · 共 38 行

function [xradar_position,x_position,x_state]=track_cv(x0,T,run,sigma)
%产生匀速运动数据
r1_randomdata=randn(1);
r2_randomdata=randn(1);
r3_randomdata=randn(1);
r1_standarddev=10;
r2_standarddev=0.05;
r3_standarddev=0.05;

F=[eye(3),diag([T T T]),zeros(3);
   zeros(3),eye(3,6);
   zeros(3,9)];

    
G_t1=diag([1 1 1]);
G_t2=diag([T T T]);
G_t3=zeros(3);
G=[G_t2;
   G_t1;
   G_t3];

v_randomdata1=randn(1);
v_randomdata2=randn(1);
v_randomdata3=randn(1);
x=x0;
x_state=x0;
x_position=[x(1),x(2),x(3)]';
xradar_position=[sqrt(x(1)^2+x(2)^2+x(3)^2),atan(x(2)/x(1)),atan(x(3)/sqrt((x(1))^2+(x(2))^2))]';
for i=2:run
    v=[v_randomdata1*sigma v_randomdata2*sigma v_randomdata3*sigma]';
    x=F*x+G*v;
    x_state=x;
    position=[x(1),x(2),x(3)]';
    x_position=[x_position  position];
    radarposition=[sqrt((x(1))^2+(x(2))^2+(x(3))^2)+r1_randomdata*r1_standarddev,atan(x(2)/x(1))+r2_randomdata*r2_standarddev,atan(x(3)/sqrt((x(1))^2+(x(2))^2))+r3_randomdata*r3_standarddev]';
    xradar_position=[xradar_position radarposition];
end