trackingEKF function for state estimation and plotting

I am trying to use the trackingEKF function to obtain the state estimate of a non-linear model with a 9x1 state vector. I am generating a trajectory of the x, y, and z position over 100 samples and storing that in a 9x100 matrix called xstorage. I am simulating measurements being taken of that initial trajectory, as stored in a 9x100 matrix called measurement. Then I want to apply an EKF (Extended Kalman Filter) to estimate the state over time from the measurement matrix, and store that in a 9x100 matrix called Est. Utimately I want to end up with a plot of 3 different curves showing the original trajectory, the measured trajectory, and the EKF estimation of the trajectory. In the code below I've tried following the documentation on these links:

https://www.mathworks.com/help/driving/ug/extended-kalman-filters.html

https://www.mathworks.com/help/driving/ref/trackingekf.html#d124e185786

But I am getting this error message:

Error using matlabshared.tracking.internal.ExtendedKalmanFilter/predict

The number of optional arguments in the call to predict does not match the number of additional arguments expected by the StateTransitionFcn. Check that all additional arguments of StateTransitionFcn are provided as optional input arguments to predict.

Error in trackingEKF/predict (line 166)

[varargout{1:nargout}] = [e-mail](filter,varargin{:});

Error in CT3D2 (line 56)

predict(EKF,T);

This is my current code:

%9x1 state vector is x=[x;y;z;xdot;ydot;zdot;wx;wy;wz]

%x, y, z are positions

%xdot, ydot, zdot are velocities

%wx, wy, wz are angular velocities

clc;

clear;

close all;

T=0.1;

samples=100;

mu = [0;0;0;0;0;0;0;0;0]; %zero mean vector

x=[50000;60000;5000;200;100;2;0;0;2.5]; %initial state estimates

sigma2=0.006; %peturbing angular velocity

%Q2 is process noise covariance matrix

Q2=sigma2*[zeros(3,3),zeros(3,3),zeros(3,3);

zeros(3,3),zeros(3,3),zeros(3,3);

zeros(3,3),zeros(3,3),eye(3,3)];

xstorage = zeros(9,samples); %store state at each time step

measurement=zeros(9,samples); %store measurements at each time step

xstorage(:,1)=x; %set first column as initial state estimates

measurement(:,1)=x; %set first column as initial state estimates

for k=2:samples

xcurrent=xstorage(:,k-1);

W=sqrt(xcurrent(7)^2+xcurrent(8)^2+xcurrent(9)^2);

c1=(cos(W*T)-1)/((W)^2);

c2=(sin(W*T))/(W);

c3=1/((W)^2) * ((sin(W*T)/(W)) - T);

d1=xcurrent(8)^2 + xcurrent(9)^2;

d2=xcurrent(7)^2 + xcurrent(9)^2;

d3=xcurrent(7)^2 + xcurrent(8)^2;

A=[c1*d1, -c2*xcurrent(9) - c1*xcurrent(7)*xcurrent(8), c2*xcurrent(8) - c1*xcurrent(7)*xcurrent(9);

c2*xcurrent(9) - c1*xcurrent(7)*xcurrent(8), c1*d2, -c2*xcurrent(7) - c1*xcurrent(8)*xcurrent(9);

-c2*xcurrent(8) - c1*xcurrent(7)*xcurrent(9), c2*xcurrent(7) - c1*xcurrent(8)*xcurrent(9), c1*d3];

B=[c3*d1, c1*xcurrent(9) - c3*xcurrent(7)*xcurrent(8), -c1*xcurrent(8) - c3*xcurrent(7)*xcurrent(9);

-c1*xcurrent(9) - c3*xcurrent(7)*xcurrent(8), c3*d2, c1*xcurrent(7) - c3*xcurrent(8)*xcurrent(9);

c1*xcurrent(8) - c3*xcurrent(7)*xcurrent(9), -c1*xcurrent(7) - c3*xcurrent(8)*xcurrent(9), c3*d3];

Phi=[eye(3,3), B, zeros(3,3); zeros(3,3), eye(3,3) + A, zeros(3,3); zeros(3,3), zeros(3,3), eye(3,3)];

xstorage(:,k)=stateUpdate(Phi,xcurrent,mu,Q2);

measurement(:,k)=measUpdate(xcurrent,sigma2);

end

Est=zeros(9,samples);

EKF = trackingEKF(State=x,StateCovariance=Q2,StateTransitionFcn=@stateUpdate,ProcessNoise=Q2,MeasurementFcn=@measUpdate,MeasurementNoise=10);

Est(:,1) = EKF.State;

for i=1:samples

predict(EKF,T);

Est(:,i)=correct(EKF,measurement(:,i),1);

end

figure();

plot3(xstorage(1,:),xstorage(2,:),xstorage(3,:),'LineWidth',1,'Color','blue');

hold on;

plot3(measurement(1,:),measurement(2,:),measurement(3,:),'LineWidth',1,'Color','green');

hold on;

plot3(Est(1,:),Est(2,:),Est(3,:),'LineWidth',1,'Color','red');

title('Coordinated Turn Model','fontsize',20,'interpreter','latex')

legend('Original','Measured','EKF','fontsize',15,'interpreter','latex');

xlabel('x','fontsize',15,'interpreter','latex');

ylabel('y','fontsize',15,'interpreter','latex');

zlabel('z','fontsize',15,'interpreter','latex');

grid on;

function nextState = stateUpdate(Phi,xcurrent,mu,Q2)

nextState=Phi*xcurrent + mvnrnd(mu,Q2)';

end

function nextMeas = measUpdate(xcurrent,sigma2)

nextMeas=xcurrent + sigma2*randn(9,1);

end

Answer 1

[Prashant Arora]

Hi Khadeja,

Your function signatures for state transition and measurement functions are incorrect. Please refer to the function signatures required by trackingEKF in the documentation for StateTransitionFcn and MeasurementFcn properties.

StateTransitionFcn - Extended Kalman filter for object tracking - MATLAB (mathworks.com)

MeasurementFcn - Extended Kalman filter for object tracking - MATLAB (mathworks.com)

In addition, you don't need to "sample" a random noise vector for extended Kalman filter estimation. An EKF only requires you to model the impact of random noise on your state transition and measurement.

Your functions may need to be updated like the following to use with trackingEKF:

function nextState = stateUpdate(xcurrent)

% Calculate Phi here.

nextState=Phi*xcurrent;

end

function nextMeas = measUpdate(xcurrent)

nextMeas=xcurrent;

end

The MeasurementNoise and ProcessNoise properties must be set to sigma2 and Q2 respectively.

[Khadeja Khan]

Hello,

Thank you for your response. Having attempted your solution it did somewhat fix the issue I was having. However, I noticed that setting MeasurementNoise=sigma2 caused the EKF estimate plot to have major divergence on the order of 10^5 in the x,y and z axes, hence why I just left it at 10. Also, by doing nextState=Phi*xcurrent; rather than nextState=Phi*xcurrent + mvnrnd(mu,Q2)'; which is what I had before, I noticed that only one type of circular trajectory was being generated and no other. Is there any way I can get it to generate different kinds of trajectories rather than the same type each time?

Thank you for your help.

[Prashant Arora]

Hi,

For simulating trajectories, I think you can reuse the model used by EKF and add additive noise on top of it.

nextState = stateUpdate(xCurrent) + mvnrnd(mu,Q)

Here, we're simply using random realizations of the model to generate simulation data.

Hope this helps

Prashant