Kalman Filter in matlab

1 vue (au cours des 30 derniers jours)
azan
azan le 5 Nov 2012
Hi everyone! I am having a video and I have to locate the position of ball using Kalman equations. Suppose the initial position(xi,yi)is known. for the first frame current position is (xi,yi).Please guide me what would be the current position(currx,curry;as in the code below)in the next frames.The piece of code is:
R=[0.2845 0.0045;0.0045 0.0455];
Hi=[1 0 0 0;0 1 0 0];
Q=0.01*eye(4);
Pe=100*eye(4);
F=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
x=zeros(569,4);
kfinite=0;
for k=1:nframes
if kfinite==0 % kalman initialization
Au=[xi;yi;0;0];
currx=Au(1);
curry=Au(2);
else
u=[x1;y1;0;0];
Au=F*x(k-1,:)'+u; % Prediction 1st equation
currx=xi
curry=yi
end
vx= currx-Au(1);
vy=curry-Au(2);
xactual=[currx;curry;vx;vy];
xactualnext=F*xactual;
ydesirednext=Hi*xactualnext;
pp=F*Pe*F' + Q; %Prediction 2nd equation
K=pp*Hi'*inv(Hi*pp*Hi' +R); %%%correction 3rd equation
Anu=Au + K*(ydesirednext-Hi*Au); %%correction 4th equation
Pe=(eye(4)-K*Hi)*pp; %%correction 5th equation
x1=Anu(1);
x2=Anu(2);
kfinite=1;
end

Réponses (1)

John Petersen
John Petersen le 23 Juil 2014
You are using +u in the prediction equation but not in the "actual" equation. Could this be your error?

Tags

Produits

Community Treasure Hunt

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

Start Hunting!

Translated by