[SOLVED] measuring sine wave distotion in matlab

N = 350;
W = [ 1 0 ; 0 1 ]; % State-error
V = [ 1 0 ; 1 0 ]; % Measurement-error
H = [ 1 0 ; 0 0 ]; % Measurement-state
Q = [ 0.001 0 ; 0 0 ]; % Process noise covariance
R = [ 0.1 0 ; 0 0.01 ]; % 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*pi/500 ];
aposteriori_state(:,1) = [ 1 ; 1*pi/500 ];
aposteriori_error(:,:,1) = [ 1 0 ; 0 1 ];
randn('state',2);
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

XX1 = abs(fft(aposteriori_state)).^2;

% to find the maximum signal power
XX2 = sort(XX1);
power = find(XX1(1:700)==XX2(700));
signal_power = XX1(power)  


harmonic_power = sum([2:N])/350  % to find the sum of harmonic power

THD_percentage= sqrt(harmonic_power)/sqrt(signal_power)