clear
close all
load data1_labsession8
G=eye(3);
% initialization
N=1000;
x0 = [20 20 pi];
x=x0;
xp=zeros(3,1);
%P=100*eye(3);
P = diag([100 100 (5/180*pi)^2]);
Xs(:,1)=x;
D(:,1)=diag(P);
% recursion
for k=1:N,
    % prediction
    xp(1) = x(1)+Ts*U(k,1)*cos(x(3));
    xp(2) = x(2)+Ts*U(k,1)*sin(x(3));
    xp(3) = x(3)+Ts*U(k,2);
    F=[1 0 -Ts*U(k,1)*sin(x(3)); 0 1 Ts*U(k,1)*cos(x(3)); 0 0 1];
    Pp = F*P*F' + G*Q*G';
        
    % correction
    dq = xp(1)^2+xp(2)^2; 
    H = [xp(1)/sqrt(dq)  xp(2)/sqrt(dq)  0; -xp(2)/dq  xp(1)/dq  0];
    K = Pp*H'*inv(H*Pp*H'+R);
    xc = xp + K*(Y(k+1,:)'-[sqrt(xp(1)^2+xp(2)^2); atan2(xp(2),xp(1))]);
    Pc = Pp*(eye(3)-H'*K');
    x=xc;
    P=Pc;
    
    Xs(:,k+1)=x;
    D(:,k+1)=diag(P);
end
for i=1:2,
 figure, plot(t,X(:,i)-Xs(i,:)','r',t,3*sqrt(D(i,:)),'b--',t,-3*sqrt(D(i,:)),'b--')
end
figure, plot(t,180/pi*(X(:,3)-Xs(3,:)'),'r',t,3*180/pi*sqrt(D(i,:)),'b--',t,-3*180/pi*sqrt(D(i,:)),'b--')
figure, plot(X(:,1),X(:,2))
hold on
plot(Xs(1,:),Xs(2,:),'r--')
%plot(Y(:,1).*cos(Y(:,2)),Y(:,1).*sin(Y(:,2)),'y')

xpu(:,1)=x0;
for k=1:N,
    % prediction
    xpu(1,k+1) = xpu(1,k)+Ts*U(k,1)*cos(xpu(3,k));
    xpu(2,k+1) = xpu(2,k)+Ts*U(k,1)*sin(xpu(3,k));
    xpu(3,k+1) = xpu(3,k)+Ts*U(k,2);
end
plot(xpu(1,:),xpu(2,:),'k')

