matlab实现扩展卡尔曼算法用于目标跟踪

% 假设两个目标都在做匀加速度的机动 % 目标跟踪 % 日期： 2011 04 17 % 基于扩展卡尔曼滤波 clear all; clc; N=400; T=100; % 仿真的时间长度 State_noise1=3; v_noise1=1.5; a_noise1=1.5; % 目标1的状态噪声方差 State_noise2=4; v_noise2=1.5; a_noise2=1; % 目标2的状态噪声方差 distance_noise=2; thera_noise=0.5/180*pi; % 传感器的观测噪声方差 F=[1 1 1 0 0 0; 0 1 1 0 0 0; 0 0 1 0 0 0; 0 0 0 1 1 1; 0 0 0 0 1 1; 0 0 0 0 0 1]; % 状态转移矩阵 True_State1(:,1)=[-1200;4;-0.5;4000;-6;1]; % 目标1的初始化状态 True_State2(:,1)=[1800;-6;1;-1000;4;-1.5]; % 目标2的初始化状态 SNoise_Matrix1=diag([State_noise1 v_noise1 a_noise1 State_noise1 v_noise1 a_noise1]); SNoise_Matrix2=diag([State_noise2 v_noise2 a_noise2 State_noise2 v_noise2 a_noise2]); MNoise_Matrix=diag([distance_noise thera_noise]); % 状态转移方程：Ture_State(:,t)=F*Ture_State(:,t-1)+v(t) % 系统观测方程：H1=sqrt(X^2+Y^2)+n1(t) H2=arctan(Y/X)+n2(t) 即目标的径向距离和观测角度 % State Transferring Processing for t=2:T % 两个目标的真实轨迹 True_State1(:,t)=F*True_State1(:,t-1)+SNoise_Matrix1*randn(6,1); True_State2(:,t)=F*True_State2(:,t-1)+SNoise_Matrix2*randn(6,1); end q11=randn(6,N); q12=randn(6,N); q21=randn(2,N); q22=randn(2,N); Q11=(q11*q11')/N; Q12=(q12*q12')/N; % 状态转移过程的噪声方差 Q21=(q21*q21')/N; Q22=(q22*q22')/N; % 观测过程的噪声方差 x=repmat(True_State1(:,1),1,N)+SNoise_Matrix1*randn(6,N); EKalman_State1(:,1)=mean(x')'; % 估计目标1初始状态 p=x-repmat(EKalman_State1(:,1),1,N); P1=(p*p')/N; % 估计目标1的状态方差 x=repmat(True_State2(:,1),1,N)+SNoise_Matrix2*randn(6,N); EKalman_State2(:,1)=mean(x')'; % 估计目标2初始状态 p=x-repmat(EKalman_State2(:,1),1,N); P2=(p*p')/N; % 估计目标2状态方差 kafang_V=zeros(2,2); % 初始化一个卡方分布的变量 % Measurement Processing Num_Date=zeros(2,2); % 初始化序列个数计算器 for t=2:T % 获得两个目标的观测值 注： 观测向量 z1 和 z2 所使用的下标的数字不对应于我们下面所标示的目标 z1(:,t)=[sqrt(True_State1(1,t)^2+True_State1(4,t)^2);atan(True_State1(4,t)/True_State1(1,t))]... +MNoise_Matrix*randn(2,1); z2(:,t)=[sqrt(True_State2(1,t)^2+True_State2(4,t)^2);atan(True_State2(4,t)/True_State2(1,t))]... +MNoise_Matrix*randn(2,1); % 分别获得两个目标的状态预测 EKF_Pre1=F*EKalman_State1(:,t-1); EKF_Pre2=F*EKalman_State2(:,t-1); % Prediction Self-connection Matrix P_Pre1=F*P1*F'+SNoise_Matrix1*Q11*SNoise_Matrix1'; P_Pre2=F*P2*F'+SNoise_Matrix2*Q12*SNoise_Matrix2'; % Obtain Jacobian Matrix of Measurement 1 H1=[EKF_Pre1(1)/sqrt(EKF_Pre1(1)^2+EKF_Pre1(4)^2) 0 0 EKF_Pre1(4)/sqrt(EKF_Pre1(1)^2+EKF_Pre1(4)^2) 0 0; -1*EKF_Pre1(4)/(EKF_Pre1(1)^2+EKF_Pre1(4)^2) 0 0 EKF_Pre1(1)/(EKF_Pre1(1)^2+EKF_Pre1(4)^2) 0 0]; H2=[EKF_Pre2(1)/sqrt(EKF_Pre2(1)^2+EKF_Pre2(4)^2) 0 0 EKF_Pre2(4)/sqrt(EKF_Pre2(1)^2+EKF_Pre2(4)^2) 0 0; -1*EKF_Pre2(4)/(EKF_Pre2(1)^2+EKF_Pre2(4)^2) 0 0 EKF_Pre2(1)/(EKF_Pre2(1)^2+EKF_Pre2(4)^2) 0 0]; % Obtain Prediction of Measurement z_Pre1=[sqrt(EKF_Pre1(1)^2+EKF_Pre1(4)^2);atan(EKF_Pre1(4)/EKF_Pre1(1))]; z_Pre2=[sqrt(EKF_Pre2(1)^2+EKF_Pre2(4)^2);atan(EKF_Pre2(4)/EKF_Pre2(1))]; % Obtain Extend Kalman Gain K1=P_Pre1*H1'*inv(H1*P_Pre1*H1'+MNoise_Matrix*Q21*MNoise_Matrix); K2=P_Pre2*H2'*inv(H2*P_Pre2*H2'+MNoise_Matrix*Q21*MNoise_Matrix); % 做两个观测数据相关处理 piancha(:,1,1)=z1(:,t)-z_Pre1; piancha(:,1,2)=z1(:,t)-z_Pre2; piancha(:,2,1)=z2(:,t)-z_Pre1; piancha(:,2,2)=z2(:,t)-z_Pre2; M_S=MNoise_Matrix*MNoise_Matrix; % 观测过程协方差矩阵 % 选择落入每个波门里面的数据个数 % 目标状态估计 EKalman_State1(:,t)=EKF_Pre1+K1*(piancha(:,1,1)); EKalman_State2(:,t)=EKF_Pre2+K2*(piancha(:,2,2)); % 估计两个目标的各自协方差 P1=(eye(6)-K1*H1)*P_Pre1; P2=(eye(6)-K2*H2)*P_Pre2; end % Show the Result of Filter figure(1); plot(True_State1(1,:),True_State1(4,:),'-r',True_State2(1,:),True_State2(4,:),'-b',EKalman_State1(1,:),EKalman_State1(4,:),'-g',EKalman_State2(1,:),EKalman_State2(4,:),'-y'); legend('Target1','Target2','跟踪目标1','跟踪目标2'); title('两个目标的跟踪'); xlabel('X轴'); ylabel('Y轴');