plz suggest modifications possible for extended kalman filter based estimation

clc;
clear all;
close all;

N = 20;
W = [ 0 0 ; 0 1 ]; % State-error
V = [ 1 0 ; 1 2 ]; % Measurement-error
H = [ 3 2 ; 0 0 ]; % Measurement-state
Q = [ 0.0001 0 ; 0 0 ]; % Process noise covariance
R = [ 0.6 0 ; 0 0.001 ]; % Measurement noise covariance

x = zeros(2,N);
apriori_state = zeros(2,N);
aposteriori_state = zeros(2,N);
apriori_error = zeros(2,2,N);
aposteriori_error = zeros(2,2,N);
z = zeros(2,N);
K = zeros(2,2,N);

x(:,1) = [ 1 ; 3.5*pi/20 ];
aposteriori_state(:,1) = [ 1 ; 3.7*pi/20 ];
aposteriori_error(:,:,1) = [ 1 0 ; 0 1 ];
randn('state',0);
n1=sqrt(Q)*[randn;randn];
n2=sqrt(R)*[randn;randn];

for i = 2:N    
    x(:,i) = [ sin(x(2,i-1)*(i-1));x(2,i-1) ] + n1;
    z(:,i) = x(:,i) + n2;
    apriori_state(:,i) = [ sin(aposteriori_state(2,i-1)*(i-1));aposteriori_state(2,i-1) ];
    Fi = [ 0 (i-1)*cos(aposteriori_state(2,i-1)*(i-1)) ; 0 1 ];
    Qi = Q;
    Ri = R;
    apriori_error(:,:,i) = Fi*aposteriori_error(:,:,i-1)*Fi' + Qi;
    K(:,:,i) = apriori_error(:,:,i)*H' / (H*apriori_error(:,:,i)*H' + Ri);
    residual(:,i) = (z(:,i) - apriori_state(:,i));
    aposteriori_state(:,i) = apriori_state(:,i) + K(:,:,i) * residual(:,i);
    aposteriori_error(:,:,i) = (eye(2) - K(:,:,i)*H) * apriori_error(:,:,i);
    error1(:,i) = apriori_state(:,i) - x(:,i);
    error2(:,i) = aposteriori_state(:,i) - x(:,i);
end
 
figure(1);
t1=2:N;
subplot(3,1,1);
plot(t1,x(1,2:N));
ylim([ -2 +2 ]);
title('Actual state');
xlabel('Time');
ylabel('Position');
grid on;
subplot(3,1,2);
plot(t1,apriori_state(1,2:N));
ylim([ -2 +2 ]);
title('Predicted state');
xlabel('Time');
ylabel('Position');
grid on;
subplot(3,1,3);
plot(t1,aposteriori_state(1,2:N));
ylim([ -2 +2 ]);
title('Updated state');
xlabel('Time');
ylabel('Position');
grid on;

figure(2);
t2=2:N;
subplot(2,1,1);
plot(t2,x(1,2:N),t2,apriori_state(1,2:N));
legend('Actual state','Predicted state');
title('Actual state and predicted state');
xlabel('Time');
ylabel('Position');
grid on;
subplot(2,1,2);
plot(t2,x(1,2:N),t2,aposteriori_state(1,2:N));
legend('Actual state','Updated state');
title('Actual state and updated state');
xlabel('Time');
ylabel('Position');
grid on;

figure(3);
t3=2:N;
subplot(2,1,1);
plot(t3,error1(1,2:N));
title('Error between actual value and predicted value');
xlabel('Time');
ylabel('Position');
grid on;
subplot(2,1,2);
plot(t3,error2(1,2:N));
title('Error between actual value and updated value');
xlabel('Time');
ylabel('Position');
grid on;

M = 2*N;
XX1 = abs(real(fft(aposteriori_state))).^2;
XX1 = sort(XX1(2:M));
XX2 = sort(XX1);
power = find(XX1==XX2(M-1));
signal_power1 = (XX2(power).^2)/2;
harmonic_power = sum(XX2(2:M-1))/(350);
signal_power = harmonic_power + signal_power1;
THD = (harmonic_power)/(signal_power)