plz suggest modifications possible for extended kalman filter based estimation

[Lekshmi B S]

Hi all,
I am doing a project on extended kalman filter estimation...and i am using sine wave as the input for the same..until now, i have found the predicted estimates and the updated estimates....then i have calculated the error by calculating the difference between these two estimates and the actual state...then the total harmonic distortion of the sine wave output in the updated estimate has been found...

now i have to adjust the parameters such that a minimum distortion can be obtained....
And again, i would like to include some modifications to this...Can anyone please suggest me the possible modifications with codes or explanations....

Hope i will get some assistance here....It is a little bit urgent......
Thank you in advance.....

- - - Updated - - -

Does anyone know how the faux algebraic Riccati technique can be incorporated with extended kalman filtering?..and for what purpose...? I read that it is a method for improving performance of EKF...& it trades off optimality for stability...
Help me if you have any other informations about this method...
Thank you....

 

[Lekshmi B S]

Hi all,
Any suggestions for the modifications possible please?...
This is the code i am using..
Also please make me know if any mistakes are there in the code..

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)