Matlab 扩展卡尔曼滤波预测更新时间
在这里输入代码我用雷达数据实现了一个跟踪运动物体的卡尔曼滤波器。我使用Matlab ADAS工具箱设计并生成了数据。生成后，我将数据输入到我在Matlab中编写的代码中（过滤器-EKF、选通-欧几里德距离、数据关联-概率数据关联、基于距离的轨迹初始化）。所以，现在在执行之后，我注意到预测并没有根据检测更新。与检测输入相比有点慢。下面是一张图片。黄色的是卡尔曼预测，红色的是探测。因此，由于后期预测和检测速度越来越快，选通结果是零检测输入到PDA块。因此，预测结果为NAN（不是数字）。为了解决这个问题，滤波器应该快速预测，这样我们就可以得到足够的检测结果来进行未来的预测。 所以，我的问题是如何做与探测相关的错误预测Matlab 扩展卡尔曼滤波预测更新时间,matlab,object-detection,prediction,kalman-filter,radar-chart,Matlab,Object Detection,Prediction,Kalman Filter,Radar Chart,在这里输入代码我用雷达数据实现了一个跟踪运动物体的卡尔曼滤波器。我使用Matlab ADAS工具箱设计并生成了数据。生成后，我将数据输入到我在Matlab中编写的代码中（过滤器-EKF、选通-欧几里德距离、数据关联-概率数据关联、基于距离的轨迹初始化）。所以，现在在执行之后，我注意到预测并没有根据检测更新。与检测输入相比有点慢。下面是一张图片。黄色的是卡尔曼预测，红色的是探测。因此，由于后期预测和检测速度越来越快，选通结果是零检测输入到PDA块。因此，预测结果为NAN（不是数字）。为了解决这个问
This is the code
This is the predict function
function [TrackList, Evaluate] = predict( Evaluate,scans,TrackList,Par,associated_data,Track_no)
persistent prev
if (~isempty(TrackList{Track_no}))
if(scans>1)
TrackList{Track_no}.X = (TrackList{Track_no}.State_trans_mtx_F)*(TrackList{Track_no}.X);
else
prev = 0;
end
Evaluate.X(scans,:) = TrackList{Track_no}.X;
TrackList{Track_no}.range_A = sqrt((TrackList{Track_no}.X(1)^2)+(TrackList{Track_no}.X(4)^2));
TrackList{Track_no}.bearing_A = atan2(TrackList{Track_no}.X(4),TrackList{Track_no}.X(1));
TrackList{Track_no}.yhat = [TrackList{Track_no}.range_A TrackList{Track_no}.X(2) TrackList{Track_no}.X(3) TrackList{Track_no}.bearing_A TrackList{Track_no}.X(5) TrackList{Track_no}.X(6)];
Evaluate.Yhat(scans,:) = TrackList{Track_no}.yhat;
TrackList{Track_no}.meas_trans_mtx_H = [cos(TrackList{Track_no}.bearing_A) 0 0 sin(TrackList{Track_no}.bearing_A) 0 0 ;
0 1 0 0 0 0;
0 0 1 0 0 0;
-sin(TrackList{Track_no}.bearing_A)/TrackList{Track_no}.range_A 0 0 cos(TrackList{Track_no}.bearing_A)/TrackList{Track_no}.range_A 0 0;
0 0 0 0 1 0;
0 0 0 0 0 1];
Evaluate.meas_trans_mtx_H(scans).m = TrackList{Track_no}.meas_trans_mtx_H;
TrackList{Track_no}.Corr_mtx_error_P = (TrackList{Track_no}.State_trans_mtx_F)*(TrackList{Track_no}.Corr_mtx_error_P)*((TrackList{Track_no}.State_trans_mtx_F)') + Par.process_cov_Q;
Evaluate.Corr_mtx_error_P(scans).m = TrackList{Track_no}.Corr_mtx_error_P ;
TrackList{Track_no}.innovation_cov_S = (TrackList{Track_no}.meas_trans_mtx_H)*(TrackList{Track_no}.Corr_mtx_error_P)*(TrackList{Track_no}.meas_trans_mtx_H') + (TrackList{Track_no}.meas_cov_R);
Evaluate.innovation_cov_S(scans).m = TrackList{Track_no}.innovation_cov_S;
associated_meas.index = 1;
associated_meas.meas = [];
for k = 1:size(associated_data,1)
[associated_meas,dis] = gating(associated_meas,associated_data(k,:),TrackList{Track_no}.yhat,Par.gateLevel,TrackList{Track_no}.innovation_cov_S);
Evaluate.dis(k + prev) = dis;
end
prev = k + prev;
Evaluate.associated_data(scans).meas = associated_meas;
TrackList{Track_no}.associated_meas.index = associated_meas.index;
TrackList{Track_no}.associated_meas.meas = associated_meas.meas;
figure(1);
polarplot(TrackList{Track_no}.yhat(4),TrackList{Track_no}.yhat(1),'o');
end
end
This is the update function
function [TrackList,Evaluate] = update(Evaluate,TrackList,Par,Track_no,scans)
% for i = 1:size(TrackList,2)
if (~isempty(TrackList{Track_no})) %#ok<*ALIGN>
TrackList{Track_no}.Kalman_gain = (TrackList{Track_no}.Corr_mtx_error_P)*(TrackList{Track_no}.meas_trans_mtx_H')/(TrackList{Track_no}.innovation_cov_S);
Evaluate.Kalman_gain(scans).m = TrackList{Track_no}.Kalman_gain ;
% PDA
[innov,TrackList] = PDA(TrackList,Track_no);
Evaluate.innov(scans).m = innov;
% Update the state and covariance estimates
TrackList{Track_no}.Kalman_gain(3,3) = 0;
TrackList{Track_no}.Kalman_gain(6,6) = 0;
TrackList{Track_no}.X = TrackList{Track_no}.X + (TrackList{Track_no}.Kalman_gain)*innov';
Evaluate.updated_X(scans,:) = TrackList{Track_no}.X;
%%
TrackList{Track_no}.Vx_save(TrackList{Track_no}.count,1) = TrackList{Track_no}.X(2);
TrackList{Track_no}.Vy_save(TrackList{Track_no}.count,1) = TrackList{Track_no}.X(5);
% calculating acceleration from velocities
if(TrackList{Track_no}.count>2)
TrackList{Track_no}.X(3) = (TrackList{Track_no}.Vx_save(TrackList{Track_no}.count,1) - TrackList{Track_no}.Vx_save(TrackList{Track_no}.count-1,1))/Par.dt;
% Ax_save(count,1) = TrackList{i}.X(3);
TrackList{Track_no}.X(6) = (TrackList{Track_no}.Vy_save(TrackList{Track_no}.count,1) - TrackList{Track_no}.Vy_save(TrackList{Track_no}.count-1,1))/Par.dt;
% Ay_save(count,1)= TrackList{i}.X(6);
end
Evaluate.updated_X_acc(scans,:) = TrackList{Track_no}.X;
TrackList{Track_no}.Kalman_out = TrackList{Track_no}.X;
TrackList{Track_no}.count = TrackList{Track_no}.count + 1;
end
end
This is the track initialization
function [TrackList] = initialize(TrackList,clustered_data,Par,cluster_id,iterate,c11,c22)
if(iterate == 1)
for id = 1:cluster_id
TrackList = add_track(TrackList,clustered_data,Par,id,c11,c22);
end
else
for track_no = 1:size(TrackList,2)
n = 0;
for id = 1:cluster_id
[t,r] = cart2pol(TrackList{track_no}.X(1),TrackList{track_no}.X(4));
dis = pdist2([r t],[clustered_data(id).meas{1,1} clustered_data(id).meas{1,4}]);
if(dis < Par.gateLevel)
TrackList{track_no}.associated_data = clustered_data(id).meas;
end
if(dis > Par.gateLevel+10)
n = n+1;
if(n == size(TrackList,2))
TrackList = add_track(TrackList,clustered_data,Par,id,c11,c22);
TrackList{track_no}.associated_data = clustered_data(id).meas;
end
end
end
end
end
end
function TrackList = add_track(TrackList,clustered_data,Par,id,c11,c22)
Track_id = size(TrackList,2) + 1;
TrackList{Track_id}.meas.range = clustered_data(id).meas{1,1};
TrackList{Track_id}.meas.bearing = clustered_data(id).meas{1,4};
TrackList{Track_id}.meas.vx = clustered_data(id).meas{1,2};
TrackList{Track_id}.meas.vy = clustered_data(id).meas{1,5};
angle = clustered_data(id).meas{1,4};
range = clustered_data(id).meas{1,1};
% TrackList{Track_id}.meas_cov_R = [(Par.meas_std_range)^2*angle 0*(Par.meas_std_dis)*(Par.meas_std_vel) 0*(Par.meas_std_dis)*(Par.meas_std_acc) 0 0 0;
% 0*(Par.meas_std_dis)*(Par.meas_std_vel) (Par.meas_std_vel)^2 (Par.meas_std_vel)*(Par.meas_std_acc)*0 0 0 0;
% 0*(Par.meas_std_dis)*(Par.meas_std_acc) 0*(Par.meas_std_vel)*(Par.meas_std_acc) (Par.meas_std_acc)^2 0 0 0;
% 0 0 0 (Par.meas_std_bearing)^2*angle 0*(Par.meas_std_dis)*(Par.meas_std_vel) 0*(Par.meas_std_dis)*(Par.meas_std_acc);
% 0 0 0 0*(Par.meas_std_dis)*(Par.meas_std_vel) (Par.meas_std_vel)^2 0*(Par.meas_std_vel)*(Par.meas_std_acc)
% 0 0 0 0*(Par.meas_std_dis)*(Par.meas_std_acc) 0*(Par.meas_std_vel)*(Par.meas_std_acc) (Par.meas_std_acc)^2];
Par.meas_std_dis_x = Par.meas_std_range*(cos(angle)^2) + (range^2)*(sin(angle)^2)*Par.meas_std_bearing;
Par.meas_std_dis_y = Par.meas_std_range*(sin(angle)^2) + (range^2)*(cos(angle)^2)*Par.meas_std_bearing;
TrackList{Track_id}.meas_cov_R = [Par.meas_std_dis_x 0*(Par.meas_std_dis)*(Par.meas_std_vel) 0*(Par.meas_std_dis)*(Par.meas_std_acc) 0 0 0;
0*(Par.meas_std_dis)*(Par.meas_std_vel) (Par.meas_std_vel)^2 (Par.meas_std_vel)*(Par.meas_std_acc)*0 0 0 0;
0*(Par.meas_std_dis)*(Par.meas_std_acc) 0*(Par.meas_std_vel)*(Par.meas_std_acc) (Par.meas_std_acc)^2 0 0 0;
0 0 0 Par.meas_std_dis_y 0*(Par.meas_std_dis)*(Par.meas_std_vel) 0*(Par.meas_std_dis)*(Par.meas_std_acc);
0 0 0 0*(Par.meas_std_dis)*(Par.meas_std_vel) (Par.meas_std_vel)^2 0*(Par.meas_std_vel)*(Par.meas_std_acc)
0 0 0 0*(Par.meas_std_dis)*(Par.meas_std_acc) 0*(Par.meas_std_vel)*(Par.meas_std_acc) (Par.meas_std_acc)^2];
% TrackList{Track_id}.meas_cov_R = [(Par.meas_std_range)^2*angle (Par.meas_std_dis)*(Par.meas_std_vel) (Par.meas_std_dis)*(Par.meas_std_acc) 0 0 0;
% (Par.meas_std_dis)*(Par.meas_std_vel) (Par.meas_std_vel)^2 (Par.meas_std_vel)*(Par.meas_std_acc)*0 0 0 0;
% (Par.meas_std_dis)*(Par.meas_std_acc) (Par.meas_std_vel)*(Par.meas_std_acc) (Par.meas_std_acc)^2 0 0 0;
% 0 0 0 (Par.meas_std_bearing)^2*angle (Par.meas_std_dis)*(Par.meas_std_vel) (Par.meas_std_dis)*(Par.meas_std_acc);
% 0 0 0 (Par.meas_std_dis)*(Par.meas_std_vel) (Par.meas_std_vel)^2 (Par.meas_std_vel)*(Par.meas_std_acc)
% 0 0 0 (Par.meas_std_dis)*(Par.meas_std_acc) (Par.meas_std_vel)*(Par.meas_std_acc) (Par.meas_std_acc)^2];
%
%TrackList{Track_id}.meas_cov_R = TrackList{Track_id}.meas_cov_R + 0.1*diag([1 0 1 1 0 1]);
TrackList{Track_id}.X = [clustered_data(id).meas{1,1}*cos(clustered_data(id).meas{1,4});clustered_data(id).meas{1,2};0;clustered_data(id).meas{1,1}*sin((clustered_data(id).meas{1,4}));clustered_data(id).meas{1,5};0];
% TrackList{Track_id}.X = [c11(id)+0.5;clustered_data(id).meas{1,2};0;c22(id)+0.5;clustered_data(id).meas{1,5};0];
TrackList{Track_id}.Corr_mtx_error_P = zeros(6,6);
TrackList{Track_id}.State_trans_mtx_F = [1 Par.dt 0.5*(Par.dt^2) 0 0 0;
0 1 Par.dt 0 0 0;
0 0 1 0 0 0;
0 0 0 1 Par.dt 0.5*(Par.dt^2);
0 0 0 0 1 Par.dt;
0 0 0 0 0 1];
TrackList{Track_id}.Vx_save = zeros(1,1);
TrackList{Track_id}.Vy_save = zeros(1,1);
TrackList{Track_id}.associated_data = clustered_data(id).meas;
TrackList{Track_id}.count = 1;
TrackList{Track_id}.death = 0;
TrackList{Track_id}.range_A = 0;
TrackList{Track_id}.bearing_A = 0;
TrackList{Track_id}.yhat = [0 0 0 0 0 0];
TrackList{Track_id}.meas_trans_mtx_H = zeros(6,6);
TrackList{Track_id}.Corr_mtx_error_P = zeros(6,6);
TrackList{Track_id}.innovation_cov_S = zeros(6,6);
TrackList{Track_id}.Kalman_gain = zeros(6,6);
TrackList{Track_id}.Vx_save = [];
TrackList{Track_id}.Vy_save = [];
TrackList{Track_id}.count = 1;
Par.process_cov_Q = [((Par.sig_ax^2)*(Par.dt^4))/4 ((Par.sig_ax^2)*(Par.dt^3))/2 ((Par.dt^2)/2)*(Par.sig_ax^2) 0 0 0;
((Par.sig_ax^2)*(Par.dt^3))/2 ((Par.sig_ax^2)*(Par.dt^2)) ((Par.sig_ax)^2)*Par.dt 0 0 0;
((Par.dt^2)/2)*(Par.sig_ax^2) ((Par.sig_ax)^2)*Par.dt (Par.sig_ax)^2 0 0 0;
0 0 0 ((Par.sig_ay^2)*(Par.dt^4))/4 ((Par.sig_ay^2)*(Par.dt^3))/2 ((Par.dt^2)/2)*(Par.sig_ay^2);
0 0 0 ((Par.sig_ay^2)*(Par.dt^3))/2 ((Par.sig_ay^2)*(Par.dt^2)) ((Par.sig_ay)^2)*Par.dt;
0 0 0 ((Par.dt^2)/2)*(Par.sig_ay^2) ((Par.sig_ay)^2)*Par.dt (Par.sig_ay)^2 ];
end
这是代码
这是预测函数
函数[跟踪列表，评估] =预测（评估，扫描，跟踪列表，PAR，关联数据，TrrkIGNO）
持续性prev
if（~isempty（TrackList{Track_no}））
如果（扫描>1）
轨迹列表{Track\u no}.X=（轨迹列表{Track\u no}.State\u trans\u mtx\u F）*（轨迹列表{Track\u no}.X）；
其他的
prev=0；
结束
Evaluate.X（scans，：）=TrackList{Track_no}.X；
TrackList{Track_no}.range_A=sqrt（（TrackList{Track_no}.X（1）^2）+（TrackList{Track_no}.X（4）^2））；
轨迹列表{Track_no}.bearing_A=atan2（轨迹列表{Track_no}.X（4），轨迹列表{Track_no}.X（1））；
TrackList{Track_no}.yhat=[TrackList{Track_no}.range_A TrackList{Track_no}.X（2）TrackList{Track_no}.X（3）TrackList{Track_no}.X（5）TrackList{Track_no}.X（6）]；
Evaluate.Yhat（scans，：）=TrackList{Track_no}.Yhat；
轨道列表{Track_no}.meas_trans_mtx_H=[cos（轨道列表{Track_no}.beating_A）0 sin（轨道列表{Track_no}.beating_A）0；
0 1 0 0 0 0;
0 0 1 0 0 0;
-sin（TrackList{Track\u no}.bearing\u A）/TrackList{Track\u no}.range\u a0 cos（TrackList{Track\u no}.bearing\u A）/TrackList{Track\u no}.range\u a0；
0 0 0 0 1 0;
0 0 0 0 0 1];
Evaluate.meas_trans_mtx_H（scans）.m=TrackList{Track_no}.meas_trans_mtx_H；
跟踪列表{Traskn}}。CrRyMtxyErrOrthP= =（TrcList{{Trrkg}}。StaseTyTrimeMTXYF）*（TrrcList{TraskIn}。CrryMtxyErrOrthPrP）*（（TrrcLoop{TrrgxNO}，StisteOx TracxMtxYf））+ PAR.Posith-Covyq；
Evaluate.Corr_mtx_error_P（scans）.m=TrackList{Track_no}.Corr_mtx_error_P；
轨道列表{Track\u no}.创新{Track\u cov\u S=（轨道列表{Track\u no}.meas\u trans\u mtx\u H）*（轨道列表{Track\u no}.Corr\u mtx\u error\u P）*（轨道列表{Track\u no}.meas\u trans mtx\u H'）+（轨道列表{Track\u no}.meas\u cov\R）；
Evaluate.innovation_cov_S（scans）.m=TrackList{Track_no}.innovation_cov_S；
相关测量指数=1；
关联_meas.meas=[]；
对于k=1：大小（关联的_数据，1）
[AsditRead Meas，DIS]＝门控（AsdithEnth-Meas，AssixDead DATA（k，，），TrcList{{Trrkg}} .yHAT，Par，gateLevel，TrcList{{Trrkg}} .
dis（k+prev）=dis；
结束
prev=k+prev；
评估关联的测量数据（扫描）。测量值=关联的测量值；
TrackList{Track\u no}.associated\u meas.index=associated\u meas.index；
轨迹列表{Track_no}.associated_meas.meas=associated_meas.meas；
图（1）；
polarplot（轨迹列表{Track_no}.yhat（4），轨迹列表{Track_no}.yhat（1），'o'）；
结束
结束
这是更新函数
函数[跟踪列表，评估] =更新（评估，跟踪列表，PAR，TrrkIGO，扫描）
%对于i=1：大小（轨迹列表，2）
if（~isempty（TrackList{Track_no}））%#ok
轨迹表{Track\u no}.Kalman_增益=（轨迹表{Track\u no}.Corr\u mtx_error\u P）*（轨迹表{Track\u no}.meas\u trans\u mtx_H'）/（轨迹表{Track\u no}.innovation\u cov\S）；
评估.Kalman_增益（扫描）.m=轨迹列表{Track_no}.Kalman_增益；
%掌上电脑
[innov，TrackList]=PDA（TrackList，Track_no）；
Evaluate.innov（scans）.m=innov；
%更新状态和协方差估计
轨迹表{Track_no}。Kalman_增益（3,3）=0；
轨迹表{Track_no}。Kalman_增益（6,6）=0；
TrackList{Track_no}.X=TrackList{Track_no}.X+（TrackList{Track_no}.Kalman_增益）*innov'；
Evaluate.updated_X（scans，：）=TrackList{Track_no}.X；
%%
TrackList{Track\u no}.Vx\u save（TrackList{Track\u no}.count，1）=TrackList{Track\u no}.X（2）；
TrackList{Track\u no}.Vy\u save（TrackList{Track\u no}.count，1）=TrackList{Track\u no}.X（5）；
%从速度计算加速度
如果（轨迹列表{Track_no}.count>2）
TrackList{Track_no}.X（3）=（TrackList{Track_no}.Vx_保存（TrackList{Track_no}.count，1）-TrackList{Track_no}.Vx_保存（TrackList{Track_no}.count-1,1））/Par.dt；
%Ax_save（count，1）=跟踪列表{i}.X（3）；
TrackList{Track_no}.X（6）=（TrackList{Track_no}.Vy_save（TrackList{Track_no}.count，1）-TrackList{Track_no}.Vy_save（TrackList{Track_no}.count-1,1））/Par.dt；
%Ay_save（count，1）=跟踪列表{i}.X（6）；
结束
Evaluate.updated_X_acc（scans，：）=TrackList{Track_no}.X；
TrackList{Track\u no}.Kalman\u out=TrackList{Track\u no}.X；
TrackList{Track\u no}.count=TrackList{Track\u no}.count+1；
结束
结束
这是轨迹初始化
函数[TrrcList]＝初始化（TrrCalb，CuxEddieStaseDATA，PAR，CultSuthID，迭代，C11，C22）
如果（迭代==1）
对于id=1:cluster\u id
TrackList= AddiTrand（TrcListar，CultEdEdTyDATA，PAR，ID，C11，C22）；
结束
其他的
对于轨道号=1：大小（轨道列表，2）
n=0；
对于id=1:cluster\u id
[t，r]=cart2pol（TrackList{track_no}.X（1），TrackList{track_no}.X（4））；
dis=pdist2（[rt]，[clustered_data（id）.meas{1,1}clustered_data（id）.meas{1,4}]）；
如果（DIS＜PAR.GATE）
TrackList{track_no}。关联的_数据=聚集的_数据（id）。meas；
结束
如果（DIS> PAR.GATELVER + 10）
n=n+1；