trackingEKF function for state estimation and plotting

13 views (last 30 days)
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:
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}] = predict@matlabshared.tracking.internal.ExtendedKalmanFilter(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

Accepted Answer

Prashant Arora
Prashant Arora on 19 Jun 2023
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.
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.
  2 Comments
Khadeja Khan
Khadeja Khan on 17 Jul 2023
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
Prashant Arora on 24 Jul 2023
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

Sign in to comment.

More Answers (0)

Categories

Find more on Tracking Filters and Motion Models in Help Center and File Exchange

Community Treasure Hunt

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

Start Hunting!