Aerospace Engineering Department Ae 484 Inertial Navigation Systems Project

Yunus Emre Arslanta 1395755

Linearized Kalman Filter Solution;

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 window clc %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

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

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

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 on plot(Std5,'r') plot(-Std5,'r')

Extended Kalman Fitler;

Matlab Code for Extended Kalman Filter;

% Initially all variables are deleted from the memeory clear all % Initially all figures are closed close all % Initially all commands are deleted from command window clc %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 %Kalman Gain is calculated Kk=(Pk*H')*inv(H*Pk*H'+R); z =[Lat_meas(k)-Xn ;Lon_meas(k)-Xe]; Xk=Kk*(z-H*Xk); %All states are corrected by adding the error states psi=Xk(1,1)+psi; Vn=Xk(2,1)+Vn ; Ve=Xk(3,1)+Ve; Xn=Xk(4,1)+Xn; Xe=Xk(5,1)+Xe; %All calculated values are assigned to arrays for plotting later psi_kal(k)=psi ; Vn_kal(k)=Vn; Ve_kal(k)=Ve; Xn_kal(k)=Xn; Xe_kal(k)=Xe;

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

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

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;

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

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

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 nonlinearities.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.