Need help to use trackingUKF function to track 3D movement of object using measurement data from Radar.

12 views (last 30 days)
Hi,
I am tring to use UKF for target tracking using measurement data from a Radar system. The output from the Radar system is range (R), azimuth angle (phi), and elevation angle (theta) of a target moving in 3D spherical coorinates (no velocity measurement). However, I don’t quite understand how I should use those three measurement data in trackingUKF().
First, to generate filter object using trackingUKF, I specified transitionfcn, measurementfcn, and gave initial state [x;vx;y;vy;z;vz] and 6x6 state covariance matrix. For now, I chose contvel and ctmeas for those options and start from [0;0;0;0;0;0] and relatively large estimation uncertanity.
I made the following simple example just to test the model. Basically, calling the ukf3d function I made from the test bench and update the state and state covariance matrix using measurement data of R, Azi, and Ele. For now, I am using constant measurement data assuming that the target is static. The plan is to change the measurement data over time to track the movement.
As I run the measurement and correction steps 100 times, I was expecting the state (x, y, z) to converge to a certain value since the measurement data is constant, but it is diverging for an unknown reason. Am I using the function wrong? Please advise me if there is anything I could try to make the function right.
Thank you for your help in advance.
%TestBench.m
%initial state
state = double([0;0;0;0;0;0]);
%initial StateCovarianceMatrix
StateCovariceMatrix = [100 0 0 0 0 0; ...
0 50 0 0 0 0; ...
0 0 100 0 0 0; ...
0 0 0 50 0 0; ...
0 0 0 0 100 0; ...
0 0 0 0 0 50];
R = 5;
Azi = 20;
Ele = -2;
StateLog = zeros;
for i = 1:100
[state, StateCovariceMatrix] = ukf3d(R, Azi, Ele, state, StateCovariceMatrix);
StateLog(i,1) = i;
StateLog(i,2) = state(1);
StateLog(i,3) = state(3);
StateLog(i,4) = state(5);
end
figure
subplot(3,1,1)
plot(StateLog(:,1), StateLog(:,2))
title('x')
subplot(3,1,2)
plot(StateLog(:,1), StateLog(:,3))
title('y')
subplot(3,1,3)
plot(StateLog(:,1), StateLog(:,4))
title('z')
%ukf3d.m
function [xpred, Ppred] = ukf3d(R, Azi, Ele, state, StateCovariceMatrix)
mp = struct("Frame","Spherical", ...
"HasAzimuth",true, ...
"HasElevation",true, ...
"HasRange",true, ...
"HasVelocity",false);
filter = trackingUKF('State', state, 'StateCovariance', StateCovariceMatrix, 'StateTransitionFcn', @constvel,'MeasurementFcn', @(state) cvmeas(state, mp));
meas = [R;Azi;Ele];
[xpred, Ppred] = predict(filter);
[xcorr, Pcorr] = correct(filter,meas);
end

Accepted Answer

Prashant Arora
Prashant Arora on 3 Feb 2023
Hi Jo,
There are two problems in the code you shared.
  1. The order of measurement according to the "cvmeas" function should be [Az;El;R] instead of [R;Az;El]
  2. You output predicted state and covariance from the ukf3d function. Then, in the next step you reinitialize the filter with the predicted state itself. Because of this, the output you logged is simply a bunch of state predictions without any correction. The reinitialization of the filter must be done with the corrected state instead.
Shown below is a working version of the code you shared.
%TestBench.m
%initial state
state = double([0;0;0;0;0;0]);
%initial StateCovarianceMatrix
StateCovariceMatrix = [100 0 0 0 0 0; ...
0 50 0 0 0 0; ...
0 0 100 0 0 0; ...
0 0 0 50 0 0; ...
0 0 0 0 100 0; ...
0 0 0 0 0 50];
R = 5;
Azi = 20;
Ele = -2;
StateLog = zeros;
mp = struct("Frame",'Spherical', ...
"HasAzimuth",true, ...
"HasElevation",true, ...
"HasRange",true, ...
"HasVelocity",false);
filter = trackingUKF('State', state, 'StateCovariance', StateCovariceMatrix, ...
'StateTransitionFcn', @constvel,'MeasurementFcn', @(state) cvmeas(state, mp));
for i = 1:100
[predState, predStateCov, state, StateCovariceMatrix] = ukf3d(R, Azi, Ele, state, StateCovariceMatrix);
% Assuming you want to log predicted state
StateLog(i,1) = i;
StateLog(i,2) = predState(1);
StateLog(i,3) = predState(3);
StateLog(i,4) = predState(5);
end
figure
subplot(3,1,1)
plot(StateLog(:,1), StateLog(:,2))
title('x')
subplot(3,1,2)
plot(StateLog(:,1), StateLog(:,3))
title('y')
subplot(3,1,3)
plot(StateLog(:,1), StateLog(:,4))
title('z')
%ukf3d.m
function [xpred, Ppred, xcorr, Pcorr] = ukf3d(R, Azi, Ele, state, StateCovariceMatrix)
mp = struct("Frame",'Spherical', ...
"HasAzimuth",true, ...
"HasElevation",true, ...
"HasRange",true, ...
"HasVelocity",false);
filter = trackingUKF('State', state, 'StateCovariance', StateCovariceMatrix, 'StateTransitionFcn', @constvel,'MeasurementFcn', @(state) cvmeas(state, mp));
meas = [Azi;Ele;R];
[xpred, Ppred] = predict(filter);
[xcorr, Pcorr] = correct(filter,meas);
end
In addition, you don't need to construct the filter every time. A faster way may be to construct the filter once and use it directly in the loop. This will look something like this:
mp = struct("Frame",'Spherical', ...
"HasAzimuth",true, ...
"HasElevation",true, ...
"HasRange",true, ...
"HasVelocity",false);
filter = trackingUKF('State', state, 'StateCovariance', StateCovariceMatrix, ...
'StateTransitionFcn', @constvel,'MeasurementFcn', @(state) cvmeas(state, mp));
for i = 1:100
meas = [Azi;Ele;R];
[xpred, Ppred] = predict(filter);
[xcorr, Pcorr] = correct(filter, meas);
StateLog(i,1) = i;
StateLog(i,2) = xpred(1);
StateLog(i,3) = xpred(3);
StateLog(i,4) = xpred(5);
end
Hope this helps,
Prashant
  4 Comments
Jo Bito
Jo Bito on 6 Feb 2023
Hi Prashant,
Thank you very much for your help! I managed to generate C program without repeating the declaration of filter object using persistent variable.
I will close this question.
Best,
Jo
Aatif Sulaimani
Aatif Sulaimani on 24 Jul 2023
Hi Prashant,
I want to convert my measurements using the measurement function instead of using Measurement Params of ObjectDetection. Is it possible? How can I do it? When I try to do it, my measurement function converts my states but the tracks still has the state of the previous measurements (not converted).
Thank you in advance,
Aatif Sulaimani

Sign in to comment.

More Answers (0)

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!