clc
clear all
t=[0:1:4000];
st = length( t );
ts =1;
Rtx = 500;
Rty = 6500;
Vtx = 10;
Vty = 2;
Rmx = 100;
Rmy = 100;
Vmx = 2;
Vmy = 3;
X = [Rtx-Rmx; Rty-Rmy;Vtx-Vmx;Vty-Vmy];
x_hat = [500;7000;0;0];
PHI = [1 0 ts 0;0 1 0 ts;0 0 1 0;0 0 0 1];
K=[0;0;0;0];
Bm = atan(4/64)+random('norm',0,0.2*pi/180);
H=[cos(Bm) -sin(Bm) 0 0];
xm = [0;0;0;0];
P = [1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
Pm= P;
for k = 2:1:4001
Rtx(k) = Rtx(k-1) + Vtx*ts;
Rty(k) = Rty(k-1) + Vty*ts;
Rmx(k) = Rmx(k-1) + Vmx*ts;
Rmy(k) = Rmy(k-1) + Vmy*ts;
Bm(k) = atan( (Rtx(k)-Rmx(k))./(Rty(k)-Rmy(k)))+random('norm',0,0.2*pi/180);
xm(:,k) = PHI*x_hat(:,k-1);
H(k,:) = [cos(Bm(k)) -sin(Bm(k)) 0 0];
% M(k,:) = [xm(2,k)./(sqrt((xm(1,k))^2+(xm(2,k))^2)) -xm(1,k)./(sqrt((xm(1,k))^2+(xm(2,k))^2)) 0 0];
Pm(:,:,k) = PHI*P(:,:,k-1)*PHI';
K(:,k) = Pm(:,:,k)*(H(k,:))'/(H(k,:)*Pm(:,:,k)*(H(k,:))'+(0.2*pi/180)^2*((xm(1,k))^2+(xm(2,k))^2));
x_hat(:,k) = (eye(4) - K(:,k)* H(k,:))* xm(:,k);
% r1(k) = sqrt((Rtx(k)-Rmx(k))^2+(Rty(k)-Rmy(k))^2);
% r2(k) = sqrt((x_hat(1,k))^2+(x_hat(2,k))^2);
P(:,:,k)=(eye(4)-K(:,k) *H(k,:))*Pm(:,:,k);
end
plot( Rtx,Rty,'-g',Rmx,Rmy,'-b',x_hat(1,:)+Rmx,x_hat(2,:)+Rmy,'-r');
grid