1, the clear % % to empty variable environment
CLC
The load shuju % % % flight track data
Re=6371393;
% real trajectories
A_R=yout (:, 1:3);
V_R=yout (:, 4:6);
P_R=yout (:, 13);
% after add noise track the results
A_ins=yout (: 10:12);
V_ins=yout (:, 13:15);
P_ins=yout (:, 16:18);
Quat=yout (: why do); % attitude quaternion
Fn=yout (:, rest); % than force under the geography department
% the noise of the relevant statistical data
Q_wg=(0.04/(57 * 3600)) ^ 2;
% gyro markov processQ_wr=(0.01/(57 * 3600)) ^ 2;
% gyro white noiseQ_wa=(1-3) e ^ 2;
% applied markov processQ=diag ([Q_wg Q_wg Q_wg, Q_wr Q_wr Q_wr, Q_wa Q_wa Q_wa]);
Tg=300 * ones (3, 1);
Ta=1000 * ones (3, 1);
% with error of GPS output signal
P_gps_sample=p_R (1:10: end, :);
N=size (p_gps_sample, 1);
P_error (:, 1:2)=30 * randn (n, 2)/Re;
30 * p_error (:, 3)=randn (n, 1); The position error %
P_gps=p_gps_sample + p_error; The position error % to join
R=diag (STD (p_error). ^ 2); R % calculation measurement noise variance
% kalman filteringTao=1; % filtering step
A_ins_sample=a_ins (1:10: end, :);
V_ins_sample=v_ins (1:10: end, :);
P_ins_sample=p_ins (1:10: end, :);
A_R_sample=a_R (1:10: end, :);
V_R_sample=v_R (1:10: end, :);
P_R_sample=p_R (1:10: end, :);
Dp=p_ins_sample - p_gps; % output INS and GPS location difference
A=a_ins_sample;
V=v_ins_sample;
P=p_ins_sample;
Quat0=quat (1:10: end, :);
Fn0=Fn (1:10: end, :);
Error_before=p_R_sample - p * 6 e6;
[Error_a, Error_v, Error_p, PP]=weizhi (Dp, v, p, quat0 Fn0, Q, R, Tg, Ta, tao); % to get the position, velocity error error estimate
A_estimate=a (1: size (Error_a, 1), :) - Error_a;
V_estimate=v (1: size (Error_v, 1), :) - Error_v;
P_estimate=p (1: size (Error_p, 1), :) - Error_p;
N=size (p_estimate, 1);
The position error % compare
Figure
Subplot (3,1,1)
Plot ((1, n), (p_R_sample (1: n, 1) - p (1: n, 1)) * 6 e6, 'k', (1, n), (p_R_sample (1: n, 1) - p_estimate (:, 1)) * 6 e6, 'r') % black line - after the filtering error in front of the red line - filtering error
Xlabel (' time, the unit 's)
Subplot (3,1,2)
Plot ((1, n), (p_R_sample (1: n, 2) - p (1: n, 2)) * 6 e6, 'k', (1, n), (p_R_sample (1: n, 2) - p_estimate (:, 2)) * 6 e6, 'r') % black line - after the filtering error in front of the red line - filtering error
Ylabel (' position error, the unit m ')
Subplot (3,1,3)
Plot ((1, n), p_R_sample (1: n, 3) - p (1: n, 3), 'k', (1, n), p_R_sample (1: n, 3) - p_estimate (:, 3), 'r') % black line - after the filtering error in front of the red line - filtering error
Xlabel (' black line - filter before the position error of the red - position error after filtering ')
% velocity error is
Figure
Subplot (3,1,1)
Plot ((1, n), v_R_sample (1: n, 1) - v (1: n, 1), 'k', (1, n), v_R_sample (1: n, 1) - v_estimate (:, 1), 'r')
Xlabel (' time, the unit 's)
Subplot (3,1,2)
The plot ((1, n), v_R_sample (1: n, 2) - v (1: n, 2), 'k', (1, n), v_R_sample (1: n, 2) - v_estimate (:, 2), 'r')
Ylabel (' velocity error, the unit of m/s)
Subplot (3,1,3)
Plot ((1, n), v_R_sample (1: n, 3) - v (1: n, 3), 'k', (1, n), v_R_sample (1: n, 3) - v_estimate (:, 3), 'r')
Xlabel (' black line - after filtering velocity error before the red - filtering velocity error ')
The position error %
Figure
Subplot (3,1,1)
Xlabel (' time, the unit 's)
The plot ((1: n), (p_R_sample (1: n, 1) - p_estimate (:, 1)) * 6370000, 'r') %
Subplot (3,1,2)
The plot ((1: n), (p_R_sample (1: n, 2) - p_estimate (:, 2)) * 6370000, 'r') %
Ylabel (' position error, the unit m ')
Subplot (3,1,3)
Plot ((1, n), p_R_sample (1: n, 3) - p_estimate (:, 3), 'r')
Xlabel (' position error after filtering)
Velocity error %
Figure
Subplot (3,1,1)
Plot ((1, n), v_R_sample (1: n, 1) - v_estimate (:, 1), 'r')
Xlabel (' time, the unit 's)
Subplot (3,1,2)
The plot ((1, n), v_R_sample (1: n, 2) - v_estimate (:, 2), 'r')
Ylabel (' velocity error, the unit of m/s)
Subplot (3,1,3)
Plot ((1, n), v_R_sample (1: n, 3) - v_estimate (:, 3), 'r')
Xlabel (' after filtering velocity error ')
2, CLC;
clear;
T=1;
% radar scan cycleN=80/T; % of total number of sampling
X=zeros(4,N); % target the real position and speed of the
X (:, 1)=,20,0,45 [0];
% target initial positionZ=zeros (2, N); % location observation
Z (:, 1)=[X (1, 1), X (3, 1));
Delta_w=1 e - 2; % process noise
Q=diag delta_w * ([0.5, 1,0.5, 1)); %
R=100 * eye (2); The mean % observation noise
F=(1, T, 0, 0, 0,1,0,0; 0, 1, T; 0,0,0,1];
H=[1,0,0,0; 0,0,1,0];
For t=2: N
X (:, t)=F (:, t - 1) * X + SQRTM randn (Q) * (4, 1); % target real trajectories
Z (:, t)=H * X (:, t) + SQRTM randn (R) * (2, 1);
% on the target of observationEnd
% Kalman Filter
Xkf=zeros (4 N);
Xkf (:, 1)=X (:, 1); % the Kalman filter state initialization
P0=eye (4); % covariance matrix initialization
For I=2: N
Xn=F * Xkf (:, I - 1);
% predictionP1=F * P0 * F '+ Q; % observation error covariance
K=p * inv (P1 H * * * H 'H' + R); % gain
Xkf (:, I)=Xn + K * (Z (: I) - H * Xn); % status update
P0=(eye (4) - K * H) * p; % filtering error covariance update
End
% % error analysisFor I=1: N
Err_Observation (I)=RMS (X (: I), Z (: I));
Err_KalmanFilter (I)=RMS (X (: I), Xkf (: I));
End
Figure
hold on; Box on;
The plot (X (1, :), X (3, :), '- k);
The plot (Z (1, :), Z (2, :), 'b');
Plot (Xkf (1, :), Xkf (3, :), '-r');
Legend (' real trajectories', 'observed trajectory,' filtering track ');
Xlabel (' abscissa/m);
Ylabel (' ordinate/m);
Figure
hold on; Box on;
Plot (Err_Observation, ' 'and' MarkerFace ', 'g');
Plot (Err_KalmanFilter, ' 'and' MarkerFace ', 'r');
Legend (' filtering error before, 'after filtering error);
Xlabel (' observation time/s);
Ylabel (' error ');
null
