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

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    

Answer 1

[Prashant Arora]

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

[Jo Bito]

Prashant,

Thank you very much for your help! Now it is working as I expected.

Regarding your comment "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:", I eventually wanted to generate C program using MATLAB coder, but it gave me error about handle class when I separated the function into ukfInit and ukf3dUpdate as follow. Is there any good way of handling filter object so that I don't need to declar trackingUKF over and over as you suggested?

Thank you for your help in advance.

function filter = ukfInit(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));
end      

function [xcorr, Pcorr, xpred, Ppred] = ukf3dUpdate(R, Azi, Ele, filter) 

meas = [Azi;Ele;R]; 

[xpred, Ppred] = predict(filter);
[xcorr, Pcorr] = correct(filter,meas);

end 


%In TestBench:
filter = ukfInit(state, stateCovariceMatrix);
[state, stateCovariceMatrix, prediction, predictedError] = ukf3dUpdate(R, Azi, Ele, filter);

[Prashant Arora]

Hi Jo,

To generate code from the filter, you can define the filter as a persistent variable, which persists across different calls to the function. For example:

function [xPred, PPred, xCorr, PCorr] = ukf3d(R, Az, El)

persistent filter
if isempty(filter)
    filter = ukfInit();
end

[xPred, PPred] = predict(filter);
[xCorr, PCorr] = correct(filter, [Az;El;R]);

end

[Jo Bito]

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]

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