введите код. Я реализовал фильтр Калмана для отслеживания движущихся объектов с использованием радиолокационных данных. Я разработал и сгенерировал данные, используя набор инструментов Matlab ADAS. После генерации я передал данные в код, который я написал в Matlab (фильтр - EKF, стробирование - евклидово расстояние, ассоциация данных - вероятностная c ассоциация данных, инициализация треков на основе расстояния). Итак, теперь после выполнения я заметил, что прогноз не обновляется в отношении обнаружений. Это немного медленно по сравнению с входами обнаружения. Вот изображение ниже. Итак, желтый - это предсказание Калмана, а красный - обнаружения. Таким образом, из-за запоздалых предсказаний и обнаружений быстрое стробирование оказывается с нулевыми обнаружениями для ввода в блок 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
Спасибо 