main_kf_error_comparison.m

比较了 各种nonlinear kalman filter 的优缺点 ukf 和各种ekf

function main_KF_error_comparison()

clear all;
close all;

load reference;
load ekf_error;
load 2n_ukf_error;
load general_ukf_error;


% abusolute value modification loop
N = 3000;

for i=1:N
    
    av_ekf_errorX(1,i) = absolute_value(ekf_errorX(1,i));
    av_ekf_errorX(2,i) = absolute_value(ekf_errorX(2,i));
    av_ekf_errorX(3,i) = absolute_value(ekf_errorX(3,i));
    
    av_2n_ukf_errorX(1,i) = absolute_value(s2n_ukf_errorX(1,i));
    av_2n_ukf_errorX(2,i) = absolute_value(s2n_ukf_errorX(2,i));
    av_2n_ukf_errorX(3,i) = absolute_value(s2n_ukf_errorX(3,i));
    
    av_general_ukf_errorX(1,i) = absolute_value(general_ukf_errorX(1,i));
    av_general_ukf_errorX(2,i) = absolute_value(general_ukf_errorX(2,i));
    av_general_ukf_errorX(3,i) = absolute_value(general_ukf_errorX(3,i));
    
end


figure();
semilogy(av_ekf_errorX(1,:),'k-');
hold on;
semilogy(av_2n_ukf_errorX(1,:),'r-');
hold on;
semilogy(av_general_ukf_errorX(1,:),'b-');
hold on;
legend('black line: ekf altitude error', 'red line: 2n ukf altitude error ','black line: general ukf altitude error');
xlabel('Time (10ms)');ylabel('altitude error (ft)');title('KF altitude error comparison');
hold off;

figure();
semilogy(av_ekf_errorX(2,:),'k-');
hold on;
semilogy(av_2n_ukf_errorX(2,:),'r-');
hold on;
semilogy(av_general_ukf_errorX(3,:),'b-');
hold on;
legend('black line: ekf velocity error', 'red line: 2n ukf velocity error ','black line: general ukf velocity error');
xlabel('Time (10ms)');ylabel('velocity error (ft/s)');title('KF velocity error comparison');
hold off;

figure();
semilogy(av_ekf_errorX(3,:),'k-');
hold on;
semilogy(av_2n_ukf_errorX(3,:),'r-');
hold on;
semilogy(av_general_ukf_errorX(3,:),'b-');
hold on;
legend('black line: ekf ball.coeff. error', 'red line: 2n ukf ball.coeff. error ','black line: general ukf ball.coeff. error');
xlabel('Time (10ms)');ylabel('ball.coeff. error(ft)');title('KF ball.coeff. error comparison');
hold off;


save KF_error_comparison av_ekf_errorX av_2n_ukf_errorX av_general_ukf_errorX;

function y = absolute_value(x)
if x<0 y=-x;
else y=x;
end