Extended Kalman Filter: Helping hand to a newbie

[Bilal_00780]

I trying to learn extended kalman filter. I am trying to simulate positions of point by angle mearsurments taken by an observer moving on a straight line. The EKF implementation does not converge. Below the MATLAB script
clear all; close all; clc;

%Generate staright line positions
T=300;
phi=[1 T 0 0; 0 1 0 0; 0 0 1 T; 0 0 0 1]; %Constant velocity motion
X=[130;-0.1;11;0.1]; %[x xdot y ydot]
for i=2:14
X=[X phi*X(:,i-1)];
end

%Calculate angles along the route, which would be passed to EKF as measured
%angles along the route
xpoint=141;
ypoint=141;
betas=atan2((ypoint-X(3,,(xpoint-X(1,);
noise=normrnd(0,sqrt((1*pi/180)),size(betas));
betas=betas+noise;
ownPos=[X(1,:);X(3,:)];

plot(X(1,:),X(3,:),'b-*');
axis equal;
hold on;
plot(xpoint,ypoint,'r*');
text(xpoint,ypoint,' Point to be found');

%Initialize variables
H=zeros(1,2,size(betas,2));
K=zeros(2,1,size(betas,2));
Xhat=zeros(2,1,size(betas,2));
X_=zeros(2,1,size(betas,2)+1);
Phat=zeros(2,2,size(betas,2));
P_=zeros(2,2,size(betas,2)+1);

%Initialize State, Pocess noise, State error covariance & measurment noise matrices

X_(:,:,1)=[135; 135]; %Initial Pricted state
Q=[0.1 0;0 0.1]; %Process noise
P_(:,:,1)=[25 0;0 25]; %Initial Pricted state error covariance
R=[(pi/180)^2]; %Measurment noise
phi=eye(2,2); %State transition matrix, since x,y are constants


Xhist=[]; %Variable to record history of state


for k=1:size(betas,2)

uk=tan(betas(k)); %Observed angle

%Calculate linearized measurment matrix
hk1=-(uk/(1+uk^2))*(1/(X_(1,1,k)-ownPos(1,k)));
hk2=(1/(1+uk^2))*(1/(X_(1,1,k)-ownPos(1,k)));
H(:,:,k)=[hk1 hk2];

%Calculate Kalman gain using linearized measurment matrix
K(:,:,k)=P_(:,:,k)*H(:,:,k)'*(inv(H(:,:,k)*P_(:,:, k)*H(:,:,k)'+R));


%Compute updated state
Xhat(:,:,k)=X_(:,:,k)+K(:,:,k)*(betas(k)-H(:,:,k)*X_(:,:,k));

%Update State Error Covariance
Phat(:,:,k)=[eye(2,2)-K(:,:,k)*H(:,:,k)]*P_(:,:,k);

%Predict state, since state is x,y of a static point predicted state
%is equal currently updated state
X_(:,:,k+1)=Xhat(:,:,k);

%Predict State Error Covariance
P_(:,:,k+1)=phi*Phat(:,:,k)*phi'+Q;


%Record history of state for plotting
Xhist=[Xhist Xhat(:,:,k)];
end

%Paint the state history
hold on;
pause;
for i=2:size(Xhist,2)
text(Xhist(1,i),Xhist(2,i),sprintf('X%2.0f',i),'Co lor','red');
pause;
end

 

[CEL]

Your error is in the update of the state. The target is constant, but the observer is moving.
The update should be:

$$x_{k+1} = Fx_k +Gu_k$$

where $$Gu_k$$ acounts for the movement of the observer.