AE454 Proje

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);


Err_vn(k)=Vn_kal(k)-Vn_t(k);


Err_ve(k)=Ve_kal(k)-Ve_t(k);

Std4(k)=sqrt(Pk(4,4));Err_xn(k)=Xn_kal(k)-Lat_t(k);


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')


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')


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')


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')


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')


ylabel('Error');

title('Estimated and True Error for North Velocity ');

hold on

plot(Std2,'r')

plot(-Std2,'r')

figure;plot(Err_ve,'m')


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')


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


Err_psi(k)=psi_kal(k)-psi_t(k);


Err_vn(k)=Vn_kal(k)-Vn_t(k);


Err_ve(k)=Ve_kal(k)-Ve_t(k);


Err_xn(k)=Xn_kal(k)-Lat_t(k);


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')


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')


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')


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')


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')


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')


ylabel('Error');

title('Estimated and True Error for Heading Angle');

hold on

plot(Std1,'r')

plot(-Std1,'r')

figure;plot(Err_vn,'m')


ylabel('Error');

title('Estimated and True Error for North Velocity ');

hold on

plot(Std2,'r')

plot(-Std2,'r')

figure;plot(Err_ve,'m')


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')


ylabel('Error');

title('Estimated and True Error for North Position');hold on

plot(Std4,'r')

plot(-Std4,'r')

figure;plot(Err_xe,'m')


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.

AE454 Proje

Documents

Transcript of AE454 Proje