trackingEKF function for state estimation and plotting
16 visualizaciones (últimos 30 días)
Mostrar comentarios más antiguos
Khadeja Khan
el 14 de Jun. de 2023
Comentada: Prashant Arora
el 24 de Jul. de 2023
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
0 comentarios
Respuesta aceptada
Prashant Arora
el 19 de Jun. de 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.
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.
2 comentarios
Prashant Arora
el 24 de Jul. de 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
Más respuestas (0)
Ver también
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!