基于matlab卡尔曼滤波目标跟踪，卡尔曼滤波目标跟踪

function OriginalDataTester() global T;%采样周期 T=1; F =[1,T,T^2,0,0,0;... 0,1,T,0,0,0;... 0,0,1,0,0,0;... 0,0,0,1,T,T^2;... 0,0,0,0,1,T;... 0,0,0,0,0,1]; H=[1,0,0,0,0,0;... 0,0,0,1,0,0]; Polar_Z0 = xlsread('F:\新型管制自动化系统核心技术\雷达真实数据\test00_01.xls',2,'E2:F4'); Z0 = zeros(3,2); for i = 1:3 [R,A] = transfer2cartestain(Polar_Z0(i,1),Polar_Z0(i,2)); Z0(i,:) = [R A]; end S_ = [0 0 1; 0 -1 1;1 -2 1]*Z0; S0 = [S_*[1;0];S_*[0;1]]; S = S0; S_K = S0; N = 100; Location = zeros(2,422); Location_K = zeros(2,422); Velocity = zeros(2,422); Velocity_K = zeros(2,422); Ascendation = zeros(2,422); Ascendation_K = zeros(2,422); Z = xlsread('F:\新型管制自动化系统核心技术\雷达真实数据\test00_01.xls',2,'E5:F426'); for i = i:N S = S0; S_K = S0; % 设置噪声水平 %极坐标下的观测噪声协方差矩阵，要修改极坐标系观测噪声水平就修改var_R，var_A即可 var_R= 80 * randn(1); var_A=0.0001*randn(1); R_ForBlue = [var_R,0;... 0,var_A]; %笛卡尔坐标下的观测噪声协方差矩阵，要修改笛卡尔坐标下的噪声水平，仅修改var_x,var_y即可 var_x = 5*randn(1); var_y = 5*randn(1); var_x2 = var_x^2; var_y2 = var_y^2; R_ForKalman =[var_x2,0;... 0,var_y2]; %过程噪声协方差矩阵（kalman和Blue都要用到） PNL = 10*randn(1);%要修改过程噪声（模型噪声）水平的话就修改这个数字。 Q=zeros(6,6); for i=1:6 Q(i,i) = PNL^2; end % 初始的状态协方差矩阵 P = Q; P_K = P; for i=1:422 z = Z(i,1:2); [S,P]=MyKarlman_Blue(S,P,F,Q,z,H,R_ForBlue); [x,y] = transfer2cartestain(Z(i,1),Z(i,2)); z_k = [x; y]; [S_K,P_K]=MyKalman(S_K,P_K,F,Q,z_k,H,R_ForKalman); Location(:,i)=Location(:,i) + [S(1);S(4)]; Velocity(:,i) = Velocity(:,i) + [S(2);S(5)]; Ascendation(:,i) = Ascendation(:,i) + [S(3);S(6)]; Location_K(:,i)=Location_K(:,i) + [S_K(1);S_K(4)]; Velocity_K(:,i) = Velocity_K(:,i) + [S_K(2);S_K(5)]; Ascendation_K(:,i) = Ascendation_K(:,i) + [S_K(3);S_K(6)]; end end Location = Location/N; Velocity = Velocity/N; Ascendation = Ascendation/N; Location_K = Location_K/N; Velocity_K = Velocity_K/N; Ascendation_K = Ascendation_K/N; figure; plot(Location(1,:),Location(2,:),'r',Location_K(1,:),Location_K(2,:),'g'); title('位置'); xlabel('X (m)'); ylabel('Y (m)'); % axis equal; figure; plot(Velocity(1,:),Velocity(2,:),'r',Velocity_K(1,:),Velocity_K(2,:),'g'); title('速度'); xlabel('X(m/s)'); ylabel('Y (m/s'); % axis equal; figure; plot(Ascendation(1,:),Ascendation(2,:),'r',Ascendation_K(1,:),Ascendation_K(2,:),'g'); title('加速度'); xlabel('X (m/s^2)'); ylabel('Y (m/s^2)'); %@subfunction %把极坐标下的量测转化为笛卡尔坐标系下 function [x,y]=transfer2cartestain(R,A) %parameter: % R:极径 % A:极角 x = R*cos(A/180*pi); y = R*sin(A/180*pi); %@subfunction %把笛卡尔坐标转化极坐标 function [R,A]=transfer2Polar(x,y) %parameter: % R:极径 % A:极角 if (x == 0 && y > 0) A = 1/2*pi; else if (x > 0 && y == 0) A = 0; else if (x > 0 && y > 0) A = atan(y/x); else if (x < 0 && y > 0) A = atan(y/x) + pi; else if (x < 0 && y == 0) A = pi; else if (x < 0 && y < 0) A = atan(y/x) + pi; else if (x == 0 && y < 0) A = 3/2*pi; else if (x > 0 && y < 0) A = atan(y/x) + 2*pi; end end end end end end end end R = sqrt(x^2 + y^2);