- The order of measurement according to the "cvmeas" function should be [Az;El;R] instead of [R;Az;El]
- 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.

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

12 views (last 30 days)

Show older comments

Jo Bito
on 3 Feb 2023

Commented: Aatif Sulaimani
on 24 Jul 2023

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

##### 0 Comments

### Accepted Answer

Prashant Arora
on 3 Feb 2023

Hi Jo,

There are two problems in the code you shared.

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

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

### More Answers (0)

### See Also

### Categories

### Community Treasure Hunt

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

Start Hunting!