clear;
clc;
K=1;
M=2;					%sensor numbers
N=500;                % Number of time steps.          `       
Q=1/3000;       % Process noise variance.
R=0.005;               % Measurement noise variance.
SNR=13;
v(1,:)=sqrt(Q)*randn(1,N);
for m=1:M
w(m,:)=sqrt(R)*randn(1,N); 
end
x(1,1)=0.5;
for t=2:N
    x(1,t)=x(1,t-1)+v(1,t-1);
end
a=zeros(1,N);   
a(1,:)=sqrt(10^(SNR/10)*R)*randn(1,N);
%*************define measurements with noises y(M,N)*************
for t=1:N
		for m=1:M
				%defind lth H matric at time t
				if ((m-1)*x(1,t))==0
					H(m,t)=(sin((-(m-1)*x(1,t))*pi)+10^(-20))/((-(m-1)*x(1,t))*pi+10^(-20));
				else
					H(m,t)=(sin((-(m-1)*x(1,t))*pi))/((-(m-1)*x(1,t))*pi);
				end
		end
		y(:,t)=H(:,t)*a(1,t);
end
y=y+w;
%EKF 
%對H矩陣進行線性化，首先對H的自變量x求導
for t=1:N
    for m=1:M
          if ((m-1)*x(1,t))==0
            HH(m,t)=cos((-m+1)*x(1,t)*pi)*(-m+1)*pi/((-m+1)*x(1,t)*pi+10^(-20))-(sin((-m+1)*x(1,t)*pi)+10^(-20))/((-m+1)*x(1,t)*pi+10^(-20))^2*(-m+1)*pi*a(1,t);
          else 
            HH(m,t)=cos((-m+1)*x(1,t)*pi)/x(1,t)-sin((-m+1)*x(1,t)*pi)/(-m+1)/x(1,t)^2/pi*a(1,t)    ;
          end
    end
end
xx(1,1)=mean(x(1,1));  %x估計初值
PP(1,1)=var(x(1,1));    %預測狀態誤差相關矩陣初值
for t=1:N-1
        for m=1:M
            x1(1,t+1)=xx(1,t)+v(1,t);
            P(1,t+1)=PP(1,t)+Q;
            S(m,t+1)=P(1,t+1)*(HH(m,t+1))'*inv(HH(m,t+1)*P(1,t+1)*(HH(m,t+1))'+R)  ;     %卡爾曼增益
            PP(1,t+1)=P(1,t+1)-S(m,t+1)*HH(m,t+1)*P(1,t+1);
            xx(1,t+1)=x1(1,t+1)+S(m,t+1)*w(m,t+1);
        end
end
figure(1)
plot(1:N,x(1,:),'b',1:N,xx(1,:),'r');
legend('True value','EKF Estimation');
ylabel('State estimation and true value','fontsize',15);
xlabel('Time step','fontsize',15);
figure(2)
plot(1:N,x(1:N)-xx(1:N));
ylabel('Error','fontsize',15);
xlabel('Time step','fontsize',15);