7/29/2019 AE454 Proje
1/28
Aerospace Engineering
Department
Ae 484Inertial Navigation
Systems
Project
Yunus Emre Arslanta
1395755
7/29/2019 AE454 Proje
2/28
7/29/2019 AE454 Proje
3/28
7/29/2019 AE454 Proje
4/28
7/29/2019 AE454 Proje
5/28
7/29/2019 AE454 Proje
6/28
7/29/2019 AE454 Proje
7/28
7/29/2019 AE454 Proje
8/28
Linearized Kalman Filter Solution;
7/29/2019 AE454 Proje
9/28
7/29/2019 AE454 Proje
10/28
7/29/2019 AE454 Proje
11/28
7/29/2019 AE454 Proje
12/28
7/29/2019 AE454 Proje
13/28
Matlab Code For Linearized Kalman Fitler;
% Initially all variables are deleted from the memeory
clear all
% Initially all figures are closed
close all
% Initially all commands are deleted from command windowclc
%All data is loaded
load 'D:\Documents and Settings\Administrator.BENIMPC\Desktop\Ae484\imu_data.mat'
%inertial measurement unit data
load 'D:\Documents and
Settings\Administrator.BENIMPC\Desktop\Ae484\Lat_Lon_GPS.mat' %Gps data
load 'D:\Documents and
Settings\Administrator.BENIMPC\Desktop\Ae484\pos_vel_att_true.mat' %True values for
comparison
%Initial Values are given
psi=0.7853;
Vn=35.3;
Ve=37.4;
Xn=537;
Xe=552;
t=360; dt=0.05;
Xk=[0;0;0;0;0];
%Covariance Matrix
Pk=[(10*pi/180)^2 0 0 0 0 ; 0 1 0 0 0;0 0 1 0 0;0 0 0 400 0;0 0 0 0 400];
%Gps measurement Error
R=[100 0;0 100];
%Conversion Matrix
H=[0 0 0 1 0;0 0 0 0 1];
%All calculations are done for all steps
for k=1:7200
%Euler Integration
psi= wz(k)*dt + psi;
Vn=(abx(k)*cos(psi)-aby(k)*sin(psi))*dt+Vn;Ve=(abx(k)*sin(psi)+aby(k)*cos(psi))*dt+Ve;
Xn=Vn*dt+Xn;
Xe=Ve*dt+Xe;
%All calculated values are assigned to arrays for plotting later
psi_ins(k)=psi;
Vn_ins(k)=Vn;
Ve_ins(k)=Ve;
Xn_ins(k)=Xn;
Xe_ins(k)=Xe;
%Kalman Gain is calculated
7/29/2019 AE454 Proje
14/28
Kk=Pk*H'*inv(H*Pk*H'+R);
% Error States are updated
z =[Lat_meas(k)-Xn ;Lon_meas(k)-Xe];
Xk=Xk+Kk*(z-H*Xk);
%All states are corrected by adding the error states to integrated values
psi_kal(k)=psi+Xk(1,1);
Vn_kal(k)=Vn+Xk(2,1);
Ve_kal(k)=Ve+Xk(3,1);
Xn_kal(k)=Xn+Xk(4,1);
Xe_kal(k)= Xe+Xk(5,1);
%Covariance is updated
Pk=(eye(5)-Kk*H)*Pk;
%Estimated and True values are calculated
Std1(k)=sqrt(Pk(1,1));
Err_psi(k)=psi_kal(k)-psi_t(k);
Std2(k)=sqrt(Pk(2,2));
Err_vn(k)=Vn_kal(k)-Vn_t(k);
Std3(k)=sqrt(Pk(3,3));
Err_ve(k)=Ve_kal(k)-Ve_t(k);
Std4(k)=sqrt(Pk(4,4));Err_xn(k)=Xn_kal(k)-Lat_t(k);
Std5(k)=sqrt(Pk(5,5));
Err_xe(k)=Xe_kal(k)-Lon_t(k);
% Error States and Covariances are projected in time
Q=[0.00001 0 0 0 0;0 0.001 0 0 0;0 0 0.001 0 0;0 0 0 0.001 0;0 0 0 0 0.001];
A=[0 0 0 0 0 ; (-abx(k)*sin(psi)-aby(k)*cos(psi)) 0 0 0 0 ; (abx(k)*cos(psi)-aby(k)*sin(psi)) 0
0 0 0 ;0 1 0 0 0 ; 0 0 1 0 0 ];
Xk=(eye(5)+A*dt)*Xk;
Pk=(eye(5)+A*dt)*Pk*(eye(5)+A*dt)'+Q;
end
%All states and errors calculations are plotted
figure;plot(psi_kal,'c')
xlabel('Time Step');
ylabel('Psi');title('Heading Angle with Linearized Kalman Filter ');
7/29/2019 AE454 Proje
15/28
hold on
plot(psi_ins)
plot(psi_t,'r')
legend('Psi Kal','Psi Meas','Psi True');
figure;plot(Vn_kal,'m')
xlabel('Time Step');ylabel('North Velocity');
title('North Velocity with Linearized Kalman Filter');
hold on
plot(Vn_ins)
plot(Vn_t,'r')
legend('Vn Kal','Vn Ins','Vn True');
figure;plot(Ve_kal,'m')
xlabel('Time Step');
ylabel('East Velocity');
title('East Velocity with Linearized Kalman Filter ');hold on
plot(Ve_ins)
plot(Ve_t,'r')
legend('Ve Kal','Ve Ins','Ve True');
figure;plot(Xn_kal,'m')
xlabel('Time Step');
ylabel('North Position');
title('North Position with Linearized Kalman Filter');
hold on
plot(Xn_ins)plot(Lat_meas,'c')
plot(Lat_t,'r')
legend('Xn Kal','Xn Ins','Xn Gps','Xn True');
figure;plot(Xe_kal,'m')
xlabel('Time Step');
ylabel('East Position');
title('East Position with Linearized Kalman Filter ');
hold on
plot(Xe_ins)
plot(Lon_meas,'c')
plot(Lon_t,'r')
legend('Xe Kal','Xe Ins','Xe Gps','Xe True');
figure;plot(Err_psi,'m')
xlabel('Time Step');
ylabel('Error');
title('Estimated and True Error for Heading Angle');
hold on
plot(Std1,'r')
plot(-Std1,'r')
7/29/2019 AE454 Proje
16/28
figure;plot(Err_vn,'m')
xlabel('Time Step');
ylabel('Error');
title('Estimated and True Error for North Velocity ');
hold on
plot(Std2,'r')
plot(-Std2,'r')
figure;plot(Err_ve,'m')
xlabel('Time Step');
ylabel('Error');
title('Estimated and True Error for East Velocity');
hold on
plot(Std3,'r')
plot(-Std3,'r')
figure;plot(Err_xn,'m')
xlabel('Time Step');ylabel('Error');
title('Estimated and True Error for North Position');
hold on
plot(Std4,'r')
plot(-Std4,'r')
figure;plot(Err_xe,'m')
xlabel('Time Step');
ylabel('Error');
title('Estimated and True Error for East Position ');
hold onplot(Std5,'r')
plot(-Std5,'r')
7/29/2019 AE454 Proje
17/28
Extended Kalman Fitler;
7/29/2019 AE454 Proje
18/28
7/29/2019 AE454 Proje
19/28
7/29/2019 AE454 Proje
20/28
7/29/2019 AE454 Proje
21/28
7/29/2019 AE454 Proje
22/28
7/29/2019 AE454 Proje
23/28
%Covariances are updated
Pk=(eye(5)-Kk*H)*Pk;
%Estimated and True values are calculated
Std1(k)=sqrt(Pk(1,1));
Err_psi(k)=psi_kal(k)-psi_t(k);
Std2(k)=sqrt(Pk(2,2));
Err_vn(k)=Vn_kal(k)-Vn_t(k);
Std3(k)=sqrt(Pk(3,3));
Err_ve(k)=Ve_kal(k)-Ve_t(k);
Std4(k)=sqrt(Pk(4,4));
Err_xn(k)=Xn_kal(k)-Lat_t(k);
Std5(k)=sqrt(Pk(5,5));
Err_xe(k)=Xe_kal(k)-Lon_t(k);
Q=[0.00001 0 0 0 0;0 0.001 0 0 0;0 0 0.001 0 0;0 0 0 0.001 0;0 0 0 0 0.001];
%A matrix is calculated with new psi value
A=[0 0 0 0 0 ; -abx(k)*sin(psi)-aby(k)*cos(psi) 0 0 0 0 ; abx(k)*cos(psi)-aby(k)*sin(psi) 0 0 0
0 ;0 1 0 0 0 ; 0 0 1 0 0 ];
% Covariances are projected in time (no need to project error states since they ar calculated at
each loop
Pk=(eye(5)+A*dt)*Pk*(eye(5)+A*dt)'+Q;
%Euler Integration
psi=wz(k)*dt + psi;Vn=(abx(k)*cos(psi)-aby(k)*sin(psi))*dt+Vn;
Ve=(abx(k)*sin(psi)+aby(k)*cos(psi))*dt+Ve;
Xn=Vn*dt+Xn;
Xe=Ve*dt+Xe;
end
%All states and errors calculations are plotted
figure;plot(psi_kal,'c')
xlabel('Time Step');
ylabel('Psi');
title('Heading Angle with Extended Kalman Filter ');
hold on
plot(psi_t,'r')
legend('Psi Kal','Psi True');
figure;plot(Vn_kal,'m')
xlabel('Time Step');
ylabel('North Velocity');title('North Velocity with Extended Kalman Filter');
7/29/2019 AE454 Proje
24/28
hold on
plot(Vn_t,'r')
legend('Vn Kal','Vn True');
figure;plot(Ve_kal,'m')
xlabel('Time Step');
ylabel('East Velocity');title('East Velocity with Extended Kalman Filter ');
hold on
plot(Ve_t,'r')
legend('Ve Kal','Ve True');
figure;plot(Xn_kal,'m')
xlabel('Time Step');
ylabel('North Position');
title('North Position with Extended Kalman Filter');
hold on
plot(Lat_meas,'c')plot(Lat_t,'r')
legend('Xn Kal','Xn Gps','Xn True');
figure;plot(Xe_kal,'m')
xlabel('Time Step');
ylabel('East Position');
title('East Position with Extended Kalman Filter ');
hold on
plot(Lon_meas,'c')
plot(Lon_t,'r')
legend('Xe Kal','Xe Gps','Xe True');
figure;plot(Err_psi,'m')
xlabel('Time Step');
ylabel('Error');
title('Estimated and True Error for Heading Angle');
hold on
plot(Std1,'r')
plot(-Std1,'r')
figure;plot(Err_vn,'m')
xlabel('Time Step');
ylabel('Error');
title('Estimated and True Error for North Velocity ');
hold on
plot(Std2,'r')
plot(-Std2,'r')
figure;plot(Err_ve,'m')
xlabel('Time Step');
ylabel('Error');
title('Estimated and True Error for East Velocity');hold on
7/29/2019 AE454 Proje
25/28
plot(Std3,'r')
plot(-Std3,'r')
figure;plot(Err_xn,'m')
xlabel('Time Step');
ylabel('Error');
title('Estimated and True Error for North Position');hold on
plot(Std4,'r')
plot(-Std4,'r')
figure;plot(Err_xe,'m')
xlabel('Time Step');
ylabel('Error');
title('Estimated and True Error for East Position ');
hold on
plot(Std5,'r')
plot(-Std5,'r')
Effect of Q;
7/29/2019 AE454 Proje
26/28
Q=[0.00001 0 0 0 0;0 0.001 0 0 0;0 0 0.001 0 0;0 0 0 0.001 0;0 0 0 0 0.001];
Q=[0.001 0 0 0 0;0 0.01 0 0 0;0 0 0.01 0 0;0 0 0 0.01 0;0 0 0 0 0.01];
Q=[0.0001 0 0 0 0;0 0.0004 0 0 0;0 0 0.0004 0 0;0 0 0 0.0004 0;0 0 0 0 0.0004];
7/29/2019 AE454 Proje
27/28
Q=[0.01 0 0 0 0;0 0.1 0 0 0;0 0 0.1 0 0;0 0 0 0.1 0;0 0 0 0 0.1];
7/29/2019 AE454 Proje
28/28
Conclusion
Two different methods for Kalman filter was analyzed.The first method ,Linearized Kalman
filter , was obtained by adding error states to open loop solution.At each step the error terms
are added so this compensates for the increasing difference between the open loop solution
and the true value.It was expected that the filtered data would have a lower error than the Gps
and the error of Imu.Both measurement techniques have characteristic errors.For example
position error obtained by Imu data is a low frequency error.However Gps data makes
oscillations.After both data was filtered in the graphs it can be seen that a smooth curve is
obtained for all states.There are some noise in psi but this is due to process error Q.
In the Extended version(used with nonlinear equations)at each step a new error state is
calculated and this new states are added to initial or previously calculated states.After
obtaining filtered solution the states are integrated using these filtered data.For example psi in
the A matrix is the filtered psi value. EKF may be applied to estimation of nonlinear
multidimensional systems with small non-linearities. This method handles well small non-
linearities.However it was seen that extended version takes longer time to complete that
calculations.
Covariances have effect on the solution.When the covariances are taken as high numbers it
was seen that initial values are noisy however as the calculations proceed smooth curves are
obtained.
Another consequence is the value of Q.As the Q (process error) decreases a smooth curve is
obtained in covariance calculations because solution is more dependent on process rather than
measurement.
After graphs were plotted it was seen that true value lies mostly between the borders drawn by
the estimated errors.And for position approximately 1-4 metre of error is achieved in both
extended and linearized version.But generally extended version is more accurate than
linearized one because in the linearization process assumptions decrease the accuracy.
Top Related