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

Question

Jo Bito 2023-2-3

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1905666-need-help-to-use-trackingukf-function-to-track-3d-movement-of-object-using-measurement-data-from-rad

评论： Aatif Sulaimani 2023-7-24

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 个评论
显示 -2更早的评论隐藏 -2更早的评论

请先登录，再进行评论。

请先登录，再回答此问题。

Answer 1

Prashant Arora 2023-2-3

在 MATLAB Online 中打开

Hi Jo,

There are two problems in the code you shared.

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.

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 个评论
显示 2更早的评论隐藏 2更早的评论

Jo Bito 2023-2-3

在 MATLAB Online 中打开

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);

Jo Bito 2023-2-6

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 2023-7-24

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

请先登录，再进行评论。

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

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

采纳的回答

4 个评论
显示 2更早的评论隐藏 2更早的评论

更多回答（0 个）

另请参阅

类别

标签

产品

版本

Community Treasure Hunt

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

0 个评论 显示 -2更早的评论隐藏 -2更早的评论

采纳的回答

4 个评论 显示 2更早的评论隐藏 2更早的评论

更多回答（0 个）

另请参阅

类别

标签

产品

版本

Community Treasure Hunt

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

4 个评论
显示 2更早的评论隐藏 2更早的评论