matlab采用卡尔曼滤波法进行运动轨迹ca cv ct模拟

% 跟踪函数 function data = simutrack_ca(x0, y0, z0, v0, a, theta , phi, sigma_ax, sigma_ay, sigma_az, sigma_r, sigma_theta, sigma_phi, sigma_v, T, N) %simutrack 仿真带加速度扰动的匀加速直线运动目标的二维航迹. % % 'x0' 目标在x方向上的初始位置 % 'y0' 目标在y方向上的初始位置 % 'z0' 目标在z方向上的初始位置 % 'v0' 目标运动方向上的初始速度 % 'a' 初始加速度(默认和速度方向一致) % 'theta' 目标初始运动方向与x轴的夹角 % 'phi' 目标初始运动方向与xy平面的夹角 % 'sigma_ax' x轴方向的随机加速度(过程噪声) % 'sigma_ay' y轴方向的随机加速度(过程噪声) % 'sigma_az' z轴方向的随机加速度(过程噪声) % 'sigma_r' 极坐标下距离的测量标准差 % 'sigma_theta' 极坐标下方位的测量标准差，单位度 % 'sigma_phi' 极坐标下俯仰的测量标准差，单位度 % 'sigma_v' 极坐标下径向速度的测量标准差 % 'T' 雷达扫描周期 % 'N' 采样点数 % 'data' 仿真得到的N点目标航迹[斜距 方位角 俯仰角 径向速度] % 角度转变为弧度 theta = theta*pi/180; phi=phi*pi/180; % xyz方向上的初始速度和初始加速度 vx0 = v0 * cos(theta)*cos(phi); vy0 = v0 * sin(theta)*cos(phi); vz0 = v0 * sin(phi); ax0 = a * cos(theta)*cos(phi); ay0 = a * sin(theta)*cos(phi); az0 = a * sin(phi); % 状态转移矩阵 F = [1 T T^2/2 0 0 0 0 0 0 ; 0 1 T 0 0 0 0 0 0 ;0 0 1 0 0 0 0 0 0;0 0 0 1 T T^2/2 0 0 0;0 0 0 0 1 T 0 0 0;0 0 0 0 0 1 0 0 0 ; 0 0 0 0 0 0 1 T T^2/2;0 0 0 0 0 0 0 1 T ;0 0 0 0 0 0 0 0 1]; % 过程噪声分布矩阵 % Q = [sigma_ax^2 0 0;0 sigma_ay^2 0;0 0 sigma_az^2]; Tao = [T^2/2 0 0;T 0 0;1 0 0;0 T^2/2 0;0 T 0;0 1 0;0 0 T^2/2;0 0 T;0 0 1]; % 构造真实航迹 X(:,1) = [x0 vx0 ax0 y0 vy0 ay0 z0 vz0 az0]'; for m = 2:N X(:,m) = F*X(:,m-1)+Tao*[sigma_ax*randn(1) sigma_ay*randn(1) sigma_az*randn(1)]'; %N个点迹的坐标 end % 极坐标下的数值 r0 = sqrt(X(1,:).^2+X(4,:).^2+X(7,:).^2); theta0 = atan(X(4,:)./X(1,:))*180/pi; phi0 = atan(X(7,:)./sqrt(X(1,:).^2+X(4,:).^2))*180/pi; vr0 = -(X(1,:).*X(2,:)+X(4,:).*X(5,:)+X(7,:).*X(8,:))./r0; %径向速度 for i=1:N if(X(1,i)>=0&&X(4,i)>0) theta0(i)=theta0(i); else if(X(1,i)>=0&&X(4,i)<0) theta0(i)=theta0(i)+360; else theta0(i)=theta0(i)+180; end end end % 加高斯观测噪声 r = r0 + sigma_r*randn(1,N); %加入量测噪声 theta = theta0 + sigma_theta*randn(1,N); phi = phi0+sigma_phi*randn(1,N); vr = vr0+sigma_v*randn(1,N); data = [r', theta',phi',vr'];