├── EKF_parameters_simulated.mat ├── LICENSE ├── Readme.txt ├── correct_states_TC.m ├── ems_data_TC_simulated.mat ├── ems_ekf_ins_gnss_main_TC.m ├── ems_symbolic_engine_TC.m └── get_cube_axis_data.m /EKF_parameters_simulated.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EmbeddedAndMultisensorSystemsLab/EMS-SymINSGNSS_TC/abaff948f8bce66da111f234fa7120185307b586/EKF_parameters_simulated.mat -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /Readme.txt: -------------------------------------------------------------------------------- 1 | To run the code, run "ems_ekf_ins_gnss_main_TC.m" 2 | 3 | The format of the INS-GNSS Tightly-Coupled dataset, "ems_data_TC_simulated.mat", is as follows: 4 | 5 | time: time measurement syncing IMU (@100hz) and GNSS (@1Hz) data 6 | the first IMU sample and the first GNSS sample are synced 7 | 8 | error_free_accel: pseudo error-free accelerometer data 9 | error_free_gyro: pseudo error-free gyroscope data 10 | sim_accel: simulated noisy accelerometer data 11 | sim_gyro: simulated noisy gyroscope data 12 | 13 | roll: ground truth roll orientation data 14 | pitch: ground pitch orientation data 15 | heading ground truth heading orientation data 16 | 17 | lat: ground truth latitude data 18 | lon: ground truth longitude data 19 | h: ground truth height data 20 | lat_noisy: noisy latitude data 21 | lon_noisy: noisy longitude data 22 | h_noisy: noisy height data 23 | 24 | north: ground truth north position data 25 | east: ground truth east position data 26 | north_noisy: noisy north position data 27 | east_noisy: noisy east position data 28 | 29 | vel: ground truth velocity data in NED frame 30 | vel_N: ground truth velocity data in ENU frame 31 | vel_noisy: noisy velocity data in NED frame 32 | vel_N_noisy: noisy velocity data in ENU frame 33 | 34 | no_of_sat: number of satellites observed at each measurement 35 | SVN: corresponding IDs of observed satellites 36 | sat_Px: satellite position data in ECEF frame along x axis in the same order as SVN 37 | sat_Py: satellite position data in ECEF frame along y axis in the same order as SVN 38 | sat_Pz: satellite position data in ECEF frame along z axis in the same order as SVN 39 | sat_Vx: satellite velocity data in ECEF frame along x axis in the same order as SVN 40 | sat_Vy: satellite velocity data in ECEF frame along y axis in the same order as SVN 41 | sat_Vz: satellite velocity data in ECEF frame along z axis in the same order as SVN 42 | sim_sat_range: simulated satellite range measurement data in the same order as SVN 43 | 44 | noiseInfo: noise parameters including: 45 | Accelerometer (XYZ) bias 46 | Accelerometer (XYZ) bias random noise std 47 | Gyrsocope (XYZ) bias 48 | Gyrsocope (XYZ) random noise std 49 | Position (NED) random noise std 50 | Velocity (NED) random noise std 51 | Receiver clock bias 52 | Range measurement random noise std 53 | -------------------------------------------------------------------------------- /correct_states_TC.m: -------------------------------------------------------------------------------- 1 | % Correct States 2 | % 3 | % This is a basic INS/GNSS Tighly-Coupled system using EKF. This is only 4 | % a DEMO with basic updates (range measurements) are applied. More advanced 5 | % updates such as range rate measurements, nonholonomic constraints, 6 | % zero-speed, and adaptive EKF are NOT implemented in this DEMO. The purpose 7 | % of the DEMO is to demonstrate the basics of EKF in a basic INS/GNSS 8 | % Tightly-Coupled fusion scenario. 9 | % 10 | % For commercial use or embdded C/C++ versions, please contact mohamed.atia@carleton.ca. 11 | % 12 | % Copyright (C) 2019, Mohamed Atia, all rights reserved. 13 | % The software is given under GNU Lesser General Public License 14 | % This program is distributed in the hope that it will be useful, 15 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | % GNU Lesser General Public License for more details. 18 | % 19 | % You should have received a copy of the GNU Lesser General Public 20 | % License along with this program. If not, see 21 | % . 22 | 23 | 24 | %--> Use the ekf state_correction_vector to correct all states 25 | lat_value(index+1) = (lat_value(index+1)*D2R + state_correction_vector(2)/(Rm_value(index+1)+alt_value(index+1)))*R2D; 26 | lon_value(index+1) = (lon_value(index+1)*D2R + state_correction_vector(1)/((Rn_value(index+1)+alt_value(index+1))*cos(lat_value(index+1)*D2R)))*R2D; 27 | C_EN_value = C_EN_fun(lat_value(index+1)*D2R , lon_value(index+1)*D2R); 28 | pos_quat_vector = dcm2quat (C_EN_value'); 29 | a_pos_value(index+1) = pos_quat_vector(1); 30 | b_pos_value(index+1) = pos_quat_vector(2); 31 | c_pos_value(index+1) = pos_quat_vector(3); 32 | d_pos_value(index+1) = pos_quat_vector(4); 33 | normalization_factor = sqrt(a_pos_value(index+1)^2 + b_pos_value(index+1)^2 + c_pos_value(index+1)^2 + d_pos_value(index+1)^2); 34 | a_pos_value(index+1) = a_pos_value(index+1)/normalization_factor; 35 | b_pos_value(index+1) = b_pos_value(index+1)/normalization_factor; 36 | c_pos_value(index+1) = c_pos_value(index+1)/normalization_factor; 37 | d_pos_value(index+1) = d_pos_value(index+1)/normalization_factor; 38 | alt_value(index+1) = alt_value(index+1) + state_correction_vector(3); 39 | ve_value(index+1) = ve_value(index+1) + state_correction_vector(4); 40 | vn_value(index+1) = vn_value(index+1) + state_correction_vector(5); 41 | vu_value(index+1) = vu_value(index+1) + state_correction_vector(6); 42 | b_value(index+1) = b_value(index+1) + state_correction_vector(7); 43 | c_value(index+1) = c_value(index+1) + state_correction_vector(8); 44 | d_value(index+1) = d_value(index+1) + state_correction_vector(9); 45 | a_value(index+1) = a_value(index+1) - ([b_value(index) c_value(index) d_value(index)]*state_correction_vector(7:9))/a_value(index); 46 | gyro_bias_x_value(index+1) = gyro_bias_x_value(index+1) + state_correction_vector(10); 47 | gyro_bias_y_value(index+1) = gyro_bias_y_value(index+1) + state_correction_vector(11); 48 | gyro_bias_z_value(index+1) = gyro_bias_z_value(index+1) + state_correction_vector(12); 49 | acc_bias_x_value(index+1) = acc_bias_x_value(index+1) + state_correction_vector(13); 50 | acc_bias_y_value(index+1) = acc_bias_y_value(index+1) + state_correction_vector(14); 51 | acc_bias_z_value(index+1) = acc_bias_z_value(index+1) + state_correction_vector(15); 52 | gyro_sf_x_value(index+1) = gyro_sf_x_value(index+1) + state_correction_vector(16); 53 | gyro_sf_y_value(index+1) = gyro_sf_y_value(index+1) + state_correction_vector(17); 54 | gyro_sf_z_value(index+1) = gyro_sf_z_value(index+1) + state_correction_vector(18); 55 | acc_sf_x_value(index+1) = acc_sf_x_value(index+1) + state_correction_vector(19); 56 | acc_sf_y_value(index+1) = acc_sf_y_value(index+1) + state_correction_vector(20); 57 | acc_sf_z_value(index+1) = acc_sf_z_value(index+1) + state_correction_vector(21); 58 | receiver_clk_bias_value(index+1) = receiver_clk_bias_value(index+1) + state_correction_vector(22); 59 | receiver_clk_drift_value(index+1) = receiver_clk_drift_value(index+1) + state_correction_vector(23); 60 | 61 | % Normalize the quaternions 62 | normalization_factor = sqrt(a_value(index+1)^2 + b_value(index+1)^2 + c_value(index+1)^2 + d_value(index+1)^2); 63 | a_value(index+1) = a_value(index+1)/normalization_factor; 64 | b_value(index+1) = b_value(index+1)/normalization_factor; 65 | c_value(index+1) = c_value(index+1)/normalization_factor; 66 | d_value(index+1) = d_value(index+1)/normalization_factor; 67 | normalization_factor = sqrt(a_pos_value(index+1)^2 + b_pos_value(index+1)^2 + c_pos_value(index+1)^2 + d_pos_value(index+1)^2); 68 | a_pos_value(index+1) = a_pos_value(index+1)/normalization_factor; 69 | b_pos_value(index+1) = b_pos_value(index+1)/normalization_factor; 70 | c_pos_value(index+1) = c_pos_value(index+1)/normalization_factor; 71 | d_pos_value(index+1) = d_pos_value(index+1)/normalization_factor; 72 | 73 | [A, p, r] = quat2angle([a_value(index+1) b_value(index+1) c_value(index+1) d_value(index+1)],'ZYX'); 74 | Euler_pitch_value(index+1) = p; 75 | Euler_roll_value(index+1) = r; 76 | Euler_heading_value(index+1) = A; 77 | 78 | Rn_value(index+1) = earth_a/sqrt(1-earth_e2*sin(lat_value(index+1)*D2R)*sin(lat_value(index+1)*D2R)); 79 | Rm_value(index+1) = earth_a*(1-earth_e2)/((1-earth_e2*sin(lat_value(index+1)*D2R)*sin(lat_value(index+1)*D2R))^(1.5)); 80 | EP_value(index+1) = (lon_value(index+1)-lon_value(1))*D2R*(Rn_value(index+1)+alt_value(index+1))*cos(lat_value(index+1)*D2R); 81 | NP_value(index+1) = (lat_value(index+1)-lat_value(1))*D2R*(Rm_value(index+1)+alt_value(index+1)); -------------------------------------------------------------------------------- /ems_data_TC_simulated.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EmbeddedAndMultisensorSystemsLab/EMS-SymINSGNSS_TC/abaff948f8bce66da111f234fa7120185307b586/ems_data_TC_simulated.mat -------------------------------------------------------------------------------- /ems_ekf_ins_gnss_main_TC.m: -------------------------------------------------------------------------------- 1 | function ems_ekf_ins_gnss_main_TC() 2 | % main module 3 | % 4 | % This is a basic INS/GNSS Tighly-Coupled system using EKF. This is only 5 | % a DEMO with basic updates (range measurements) are applied. More advanced 6 | % updates such as range rate measurements, nonholonomic constraints, 7 | % zero-speed, and adaptive EKF are NOT implemented in this DEMO. The purpose 8 | % of the DEMO is to demonstrate the basics of EKF in a basic INS/GNSS 9 | % Tightly-Coupled fusion scenario. 10 | % 11 | % For commercial use or embdded C/C++ versions, please contact mohamed.atia@carleton.ca. 12 | % 13 | % Copyright (C) 2019, Mohamed Atia, all rights reserved. 14 | % The software is given under GNU Lesser General Public License 15 | % This program is distributed in the hope that it will be useful, 16 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | % GNU Lesser General Public License for more details. 19 | % 20 | % You should have received a copy of the GNU Lesser General Public 21 | % License along with this program. If not, see 22 | % . 23 | 24 | 25 | set(0,'DefaultFigureWindowStyle','docked'); 26 | opengl('save', 'software'); 27 | clear; 28 | close all; 29 | fclose all; 30 | clc; 31 | 32 | 33 | %--> Load EKF parameters 34 | load('EKF_parameters_simulated'); 35 | 36 | %--> Call symbolic mechanization equation definition script 37 | ems_symbolic_engine_TC; 38 | 39 | %--> Enable/Disable animation of results 40 | enable_animation = 1; 41 | 42 | %--> Converting symbolic functions into real-valued functions 43 | C_LB_from_Euler_fun = matlabFunction(C_LB_from_Euler); 44 | w_L_IL_fun = matlabFunction(w_L_IL); 45 | a_dot_fun = matlabFunction(a_dot); 46 | b_dot_fun = matlabFunction(b_dot); 47 | c_dot_fun = matlabFunction(c_dot); 48 | d_dot_fun = matlabFunction(d_dot); 49 | C_LB_from_quat_fun = matlabFunction(C_LB_from_quat); 50 | C_EN_fun = matlabFunction(C_EN); 51 | C_EL_fun = matlabFunction(C_EL); 52 | w_N_EN_fun = matlabFunction(w_N_EN); 53 | w_N_IE_fun = matlabFunction(w_N_IE); 54 | V_N_dot_fun = matlabFunction(V_N_dot); 55 | pos_quat_dot_fun_EN = matlabFunction(pos_quat_dot_EN); 56 | pos_quat_dot_fun_EL = matlabFunction(pos_quat_dot_EL); 57 | C_EN_from_pos_quat_fun = matlabFunction(C_EN_from_pos_quat); 58 | C_EL_from_pos_quat_fun = matlabFunction(C_EL_from_pos_quat); 59 | pos_quat_from_lat_lon_fun = matlabFunction(pos_quat_from_lat_lon); 60 | w_L_IE_fun = matlabFunction(w_L_IE); 61 | gravity_fun = matlabFunction(g); 62 | F_fun = matlabFunction(F); 63 | G_fun = matlabFunction(G); 64 | H_row_range_fun = matlabFunction(H_row_range); 65 | 66 | %--> Earth ellipsoid shape parameters 67 | earth_a = 6378137; 68 | earth_f = 1/298.257223563; 69 | earth_b = earth_a*(1-earth_f); 70 | earth_e2 = 1-(earth_b^2)/(earth_a^2); 71 | we_value = 2*pi/(24*60*60); 72 | 73 | %--> Load the dataset 74 | load('ems_data_TC_simulated'); 75 | 76 | %--> Plot IMU and reference navigation data 77 | figure; 78 | plot(ems_data.time,ems_data.sim_gyro(:,1)*R2D,'r', 'LineWidth',1); hold on; grid on; set(gca,'FontSize',12); 79 | plot(ems_data.time,ems_data.sim_gyro(:,2)*R2D,'g', 'LineWidth',1); 80 | plot(ems_data.time,ems_data.sim_gyro(:,3)*R2D,'b', 'LineWidth',1); legend({'gyro x','gyro y','gyro z'}, 'FontSize', 12); title('Simulated IMU - Gyroscope'); xlabel('Time (s)');ylabel('Rotation Rate (deg/s)'); 81 | 82 | figure; 83 | plot(ems_data.time,ems_data.sim_accel(:,1),'r', 'LineWidth',1); hold on; grid on; set(gca,'FontSize',12); 84 | plot(ems_data.time,ems_data.sim_accel(:,2),'g', 'LineWidth',1); 85 | plot(ems_data.time,ems_data.sim_accel(:,3),'b', 'LineWidth',1); legend({'accel x','accel y','accel z'}, 'FontSize', 12); title('Simulated IMU - Accelerometer'); xlabel('Time (s)');ylabel('Acceleration (m/s^2)'); 86 | 87 | figure; 88 | plot(ems_data.time,ems_data.roll*R2D,'r', 'LineWidth',2); hold on; grid on; set(gca,'FontSize',12); 89 | plot(ems_data.time,ems_data.pitch*R2D,'g', 'LineWidth',2); 90 | plot(ems_data.time,ems_data.heading*R2D,'b', 'LineWidth',2); legend({'Roll','Pitch','Heading'}, 'FontSize', 12); title('Reference 3D Orientation'); xlabel('Time (s)');ylabel('Orientation (deg)'); 91 | 92 | figure; 93 | plot(ems_data.time,ems_data.vel(:,1),'r', 'LineWidth',2); hold on; grid on; set(gca,'FontSize',12); 94 | plot(ems_data.time,ems_data.vel(:,2),'g', 'LineWidth',2); 95 | plot(ems_data.time,ems_data.vel(:,3),'b', 'LineWidth',2); legend({'V_n','V_e','V_d'}, 'FontSize', 12); title('Reference 3D Velocity'); xlabel('Time (s)');ylabel('Velocity (m/s)'); 96 | 97 | figure; 98 | plot(ems_data.time,ems_data.north,'r', 'LineWidth',2);hold on; grid on;set(gca,'FontSize',12); 99 | plot(ems_data.time,ems_data.east,'g', 'LineWidth',2); 100 | plot(ems_data.time,-ems_data.h,'b', 'LineWidth',2); legend({'North','East','Down'}, 'FontSize', 12); title('Reference 3D Position'); xlabel('Time (s)'); ylabel('Position (m)'); 101 | 102 | figure; 103 | plot(ems_data.east, ems_data.north, 'LineWidth',2); title('Reference 2D Position'); xlabel('East (m)'); ylabel('North (m)'); grid on; set(gca,'FontSize',12); 104 | 105 | %--> Initialization 106 | num_of_samples = length(ems_data.time); 107 | sampling_period_sec = mean(diff(ems_data.time)); 108 | sampling_freq = round(1/sampling_period_sec); 109 | 110 | %--> Initialize vectors sizes 111 | time_vector_sec = zeros(num_of_samples,1); 112 | accel_x = zeros(num_of_samples,1); 113 | accel_y = zeros(num_of_samples,1); 114 | accel_z = zeros(num_of_samples,1); 115 | gyro_x = zeros(num_of_samples,1); 116 | gyro_y = zeros(num_of_samples,1); 117 | gyro_z = zeros(num_of_samples,1); 118 | a_value = zeros(num_of_samples,1); 119 | b_value = zeros(num_of_samples,1); 120 | c_value = zeros(num_of_samples,1); 121 | d_value = zeros(num_of_samples,1); 122 | Euler_pitch_value = zeros(num_of_samples,1); 123 | Euler_roll_value = zeros(num_of_samples,1); 124 | Euler_heading_value = zeros(num_of_samples,1); 125 | ve_value = zeros(num_of_samples,1); 126 | vn_value = zeros(num_of_samples,1); 127 | vu_value = zeros(num_of_samples,1); 128 | a_pos_value = zeros(num_of_samples,1); 129 | b_pos_value = zeros(num_of_samples,1); 130 | c_pos_value = zeros(num_of_samples,1); 131 | d_pos_value = zeros(num_of_samples,1); 132 | lat_value = zeros(num_of_samples,1); 133 | lon_value = zeros(num_of_samples,1); 134 | alt_value = zeros(num_of_samples,1); 135 | Rn_value = zeros(num_of_samples,1); 136 | Rm_value = zeros(num_of_samples,1); 137 | EP_value = zeros(num_of_samples,1); 138 | NP_value = zeros(num_of_samples,1); 139 | 140 | gyro_bias_x_value = zeros(num_of_samples,1); 141 | gyro_bias_y_value = zeros(num_of_samples,1); 142 | gyro_bias_z_value = zeros(num_of_samples,1); 143 | acc_bias_x_value = zeros(num_of_samples,1); 144 | acc_bias_y_value = zeros(num_of_samples,1); 145 | acc_bias_z_value = zeros(num_of_samples,1); 146 | 147 | gyro_sf_x_value = zeros(num_of_samples,1); 148 | gyro_sf_y_value = zeros(num_of_samples,1); 149 | gyro_sf_z_value = zeros(num_of_samples,1); 150 | acc_sf_x_value = zeros(num_of_samples,1); 151 | acc_sf_y_value = zeros(num_of_samples,1); 152 | acc_sf_z_value = zeros(num_of_samples,1); 153 | 154 | g_value = zeros(num_of_samples,1); 155 | 156 | gyro_rw_stdv_x_value = zeros(num_of_samples,1); 157 | gyro_rw_stdv_y_value = zeros(num_of_samples,1); 158 | gyro_rw_stdv_z_value = zeros(num_of_samples,1); 159 | gyro_bias_gauss_markov_stdv_x_value = zeros(num_of_samples,1); 160 | gyro_bias_gauss_markov_stdv_y_value = zeros(num_of_samples,1); 161 | gyro_bias_gauss_markov_stdv_z_value = zeros(num_of_samples,1); 162 | gyro_bias_x_time_cnst_value = zeros(num_of_samples,1); 163 | gyro_bias_y_time_cnst_value = zeros(num_of_samples,1); 164 | gyro_bias_z_time_cnst_value = zeros(num_of_samples,1); 165 | gyro_sf_gauss_markov_stdv_x_value = zeros(num_of_samples,1); 166 | gyro_sf_gauss_markov_stdv_y_value = zeros(num_of_samples,1); 167 | gyro_sf_gauss_markov_stdv_z_value = zeros(num_of_samples,1); 168 | gyro_sf_x_time_cnst_value = zeros(num_of_samples,1); 169 | gyro_sf_y_time_cnst_value = zeros(num_of_samples,1); 170 | gyro_sf_z_time_cnst_value = zeros(num_of_samples,1); 171 | 172 | acc_rw_stdv_x_value = zeros(num_of_samples,1); 173 | acc_rw_stdv_y_value = zeros(num_of_samples,1); 174 | acc_rw_stdv_z_value = zeros(num_of_samples,1); 175 | acc_bias_gauss_markov_stdv_x_value = zeros(num_of_samples,1); 176 | acc_bias_gauss_markov_stdv_y_value = zeros(num_of_samples,1); 177 | acc_bias_gauss_markov_stdv_z_value = zeros(num_of_samples,1); 178 | acc_bias_x_time_cnst_value = zeros(num_of_samples,1); 179 | acc_bias_y_time_cnst_value = zeros(num_of_samples,1); 180 | acc_bias_z_time_cnst_value = zeros(num_of_samples,1); 181 | acc_sf_gauss_markov_stdv_x_value = zeros(num_of_samples,1); 182 | acc_sf_gauss_markov_stdv_y_value = zeros(num_of_samples,1); 183 | acc_sf_gauss_markov_stdv_z_value = zeros(num_of_samples,1); 184 | acc_sf_x_time_cnst_value = zeros(num_of_samples,1); 185 | acc_sf_y_time_cnst_value = zeros(num_of_samples,1); 186 | acc_sf_z_time_cnst_value = zeros(num_of_samples,1); 187 | 188 | a_pos_rw_stdv_value = zeros(num_of_samples,1); 189 | b_pos_rw_stdv_value = zeros(num_of_samples,1); 190 | c_pos_rw_stdv_value = zeros(num_of_samples,1); 191 | d_pos_rw_stdv_value = zeros(num_of_samples,1); 192 | alt_rw_stdv_value = zeros(num_of_samples,1); 193 | 194 | receiver_clk_bias_value = zeros(num_of_samples,1); 195 | receiver_clk_bias_stdv_value = zeros(num_of_samples,1); 196 | receiver_clk_drift_value = zeros(num_of_samples,1); 197 | receiver_clk_drift_stdv_value = zeros(num_of_samples,1); 198 | 199 | wg_noise_value = zeros(num_of_samples,1); 200 | 201 | %--> Initialize the state 202 | Euler_roll_value(1) = ems_data.roll(1); 203 | Euler_pitch_value(1) = ems_data.pitch(1); 204 | Euler_heading_value(1) = ems_data.heading(1); 205 | attitude_quat_value = angle2quat(Euler_heading_value(1),Euler_pitch_value(1),Euler_roll_value(1),'ZYX'); 206 | C_BL_value = angle2dcm(Euler_heading_value(1),Euler_pitch_value(1),Euler_roll_value(1),'ZYX'); 207 | C_LB_value = C_BL_value'; 208 | a_value(1) = attitude_quat_value(1); 209 | b_value(1) = attitude_quat_value(2); 210 | c_value(1) = attitude_quat_value(3); 211 | d_value(1) = attitude_quat_value(4); 212 | lat_value(1) = ems_data.lat(1)*R2D; 213 | lon_value(1) = ems_data.lon(1)*R2D; 214 | alt_value(1) = ems_data.h(1); 215 | Rn_value(1) = earth_a/sqrt(1-earth_e2*sin(lat_value(1)*D2R)*sin(lat_value(1)*D2R)); 216 | Rm_value(1) = earth_a*(1-earth_e2)/((1-earth_e2*sin(lat_value(1)*D2R)*sin(lat_value(1)*D2R))^(1.5)); 217 | EP_value(1) = ems_data.east(1); 218 | NP_value(1) = ems_data.north(1); 219 | g_value(1) = gravity_fun(Rm_value(1),Rn_value(1),alt_value(1),lat_value(1)); 220 | C_EN_value = C_EN_fun(lat_value(1)*D2R , lon_value(1)*D2R); 221 | pos_quat_vector = dcm2quat (C_EN_value'); 222 | a_pos_value(1) = pos_quat_vector(1)/sqrt(sum(pos_quat_vector.^2)); 223 | b_pos_value(1) = pos_quat_vector(2)/sqrt(sum(pos_quat_vector.^2)); 224 | c_pos_value(1) = pos_quat_vector(3)/sqrt(sum(pos_quat_vector.^2)); 225 | d_pos_value(1) = pos_quat_vector(4)/sqrt(sum(pos_quat_vector.^2)); 226 | ve_value(1) = ems_data.vel_N(1,1); 227 | vn_value(1) = ems_data.vel_N(1,2); 228 | vu_value(1) = ems_data.vel_N(1,3); 229 | 230 | %--> Initialize biases 231 | gyro_bias_x_value(1) = 0.0; 232 | gyro_bias_y_value(1) = 0.0; 233 | gyro_bias_z_value(1) = 0.0; 234 | acc_bias_x_value(1) = 0.0; 235 | acc_bias_y_value(1) = 0.0; 236 | acc_bias_z_value(1) = 0.0; 237 | 238 | %--> Initialize scale factors 239 | gyro_sf_x_value(1) = 0.0; 240 | gyro_sf_y_value(1) = 0.0; 241 | gyro_sf_z_value(1) = 0.0; 242 | acc_sf_x_value(1) = 0.0; 243 | acc_sf_y_value(1) = 0.0; 244 | acc_sf_z_value(1) = 0.0; 245 | 246 | %--> Noise is to set to zero in INS prediction 247 | wg_noise_value(1) = 0.0; 248 | 249 | %--> Position system noise in Q matrix 250 | a_pos_rw_stdv_value(1) = 0.0; 251 | b_pos_rw_stdv_value(1) = 0.0; 252 | c_pos_rw_stdv_value(1) = 0.0; 253 | d_pos_rw_stdv_value(1) = 0.0; 254 | alt_rw_stdv_value(1) = 0.0; 255 | 256 | %--> Accelerometer System noise and Gauss-Markov model parameters 257 | acc_rw_stdv_x_value(1) = parameters(1); 258 | acc_rw_stdv_y_value(1) = parameters(2); 259 | acc_rw_stdv_z_value(1) = parameters(3); 260 | acc_bias_gauss_markov_stdv_x_value (1) = parameters(4); 261 | acc_bias_gauss_markov_stdv_y_value (1) = parameters(5); 262 | acc_bias_gauss_markov_stdv_z_value (1) = parameters(6); 263 | acc_bias_x_time_cnst_value(1) = parameters(7); 264 | acc_bias_y_time_cnst_value(1) = parameters(8); 265 | acc_bias_z_time_cnst_value(1) = parameters(9); 266 | acc_sf_gauss_markov_stdv_x_value (1) = parameters(10); 267 | acc_sf_gauss_markov_stdv_y_value (1) = parameters(11); 268 | acc_sf_gauss_markov_stdv_z_value (1) = parameters(12); 269 | acc_sf_x_time_cnst_value(1) = parameters(13); 270 | acc_sf_y_time_cnst_value(1) = parameters(14); 271 | acc_sf_z_time_cnst_value(1) = parameters(15); 272 | 273 | %--> Gyroscope System noise and Gauss-Markov model parameters 274 | gyro_rw_stdv_x_value(1) = parameters(16); 275 | gyro_rw_stdv_y_value(1) = parameters(17); 276 | gyro_rw_stdv_z_value(1) = parameters(18); 277 | gyro_bias_gauss_markov_stdv_x_value (1) = parameters(19); 278 | gyro_bias_gauss_markov_stdv_y_value (1) = parameters(20); 279 | gyro_bias_gauss_markov_stdv_z_value (1) = parameters(21); 280 | gyro_bias_x_time_cnst_value(1) = parameters(22); 281 | gyro_bias_y_time_cnst_value(1) = parameters(23); 282 | gyro_bias_z_time_cnst_value(1) = parameters(24); 283 | gyro_sf_gauss_markov_stdv_x_value (1) = parameters(25); 284 | gyro_sf_gauss_markov_stdv_y_value (1) = parameters(26); 285 | gyro_sf_gauss_markov_stdv_z_value (1) = parameters(27); 286 | gyro_sf_x_time_cnst_value(1) = parameters(28); 287 | gyro_sf_y_time_cnst_value(1) = parameters(29); 288 | gyro_sf_z_time_cnst_value(1) = parameters(30); 289 | 290 | %--> Receiver clock errors parameters 291 | receiver_clk_bias_stdv_value(1) = parameters(31); 292 | receiver_clk_drift_stdv_value(1) = parameters(32); 293 | 294 | %--> Initialize state error covariances 295 | east_pos_error_covariance(1) = parameters(33); 296 | north_pos_error_covariance(1) = parameters(34); 297 | alt_error_covariance(1) = parameters(35); 298 | ve_error_covariance(1) = parameters(36); 299 | vn_error_covariance(1) = parameters(37); 300 | vu_error_covariance(1) = parameters(38); 301 | b_error_covariance(1) = parameters(39); 302 | c_error_covariance(1) = parameters(40); 303 | d_error_covariance(1) = parameters(41); 304 | gyro_bias_x_error_covariance(1) = parameters(42); 305 | gyro_bias_y_error_covariance(1) = parameters(43); 306 | gyro_bias_z_error_covariance(1) = parameters(44); 307 | acc_bias_x_error_covariance(1) = parameters(45); 308 | acc_bias_y_error_covariance(1) = parameters(46); 309 | acc_bias_z_error_covariance(1) = parameters(47); 310 | gyro_sf_x_error_covariance(1) = parameters(48); 311 | gyro_sf_y_error_covariance(1) = parameters(49); 312 | gyro_sf_z_error_covariance(1) =parameters(50); 313 | acc_sf_x_error_covariance(1) = parameters(51); 314 | acc_sf_y_error_covariance(1) = parameters(52); 315 | acc_sf_z_error_covariance(1) = parameters(53); 316 | receiver_clk_bias_error_covariance(1) =parameters(54); 317 | receiver_clk_drift_error_covariance(1) =parameters(55); 318 | 319 | P = diag([ 320 | east_pos_error_covariance(1); 321 | north_pos_error_covariance(1); 322 | alt_error_covariance(1); 323 | ve_error_covariance(1); 324 | vn_error_covariance(1); 325 | vu_error_covariance(1); 326 | b_error_covariance(1); 327 | c_error_covariance(1); 328 | d_error_covariance(1); 329 | gyro_bias_x_error_covariance(1); 330 | gyro_bias_y_error_covariance(1); 331 | gyro_bias_z_error_covariance(1); 332 | acc_bias_x_error_covariance(1); 333 | acc_bias_y_error_covariance(1); 334 | acc_bias_z_error_covariance(1); 335 | gyro_sf_x_error_covariance(1); 336 | gyro_sf_y_error_covariance(1); 337 | gyro_sf_z_error_covariance(1); 338 | acc_sf_x_error_covariance(1); 339 | acc_sf_y_error_covariance(1); 340 | acc_sf_z_error_covariance(1); 341 | receiver_clk_bias_error_covariance(1); 342 | receiver_clk_drift_error_covariance(1) 343 | ]); 344 | 345 | %--> Draw a 3D Aircraft for animation 346 | X = 60;Y = 15;Z = 5; 347 | origin = [X/2 Y/2 Z/2]; 348 | initial_vert = ... 349 | [X Y 0; %(1) 350 | 0 Y 0; %(2) 351 | 0 Y Z; %(3) 352 | X Y Z; %(4) 353 | 0 0 Z; %(5) 354 | X 0 Z; %(6) 355 | X 0 0; %(7) 356 | 0 0 0; %(8) 357 | 1.3*X Y/2 0; %(9) 358 | 1.3*X Y/2 0.5*Z; %(10) 359 | ]; 360 | 361 | for p = 1:length(initial_vert) 362 | initial_vert(p,:) = initial_vert(p,:) - origin; 363 | end 364 | 365 | CubePoints = [initial_vert(:,1),initial_vert(:,2),initial_vert(:,3)]; 366 | [ CubeXData , CubeYData , CubeZData ] = get_cube_axis_data(initial_vert); 367 | faces = [1 2 3 4; 4 3 5 6; 6 7 8 5; 1 2 8 7; 6 7 1 4; 2 3 5 8;1 9 10 4;4 10 6 6;6 10 9 7;1 9 7 7]; 368 | 369 | %--> Main processing loop 370 | for index = 1:length(ems_data.time)-1 371 | 372 | time_vector_sec(index) = ems_data.time(index); 373 | 374 | %--> Read IMU data 375 | accel_x(index) = ems_data.sim_accel(index,1); 376 | accel_y(index) = ems_data.sim_accel(index,2); 377 | accel_z(index) = ems_data.sim_accel(index,3); 378 | 379 | gyro_x(index) = ems_data.sim_gyro(index,1); 380 | gyro_y(index) = ems_data.sim_gyro(index,2); 381 | gyro_z(index) = ems_data.sim_gyro(index,3); 382 | 383 | %--> Calculate Earth Rotation Rate, Transport Rate, and Corriollis Effect 384 | w_N_EN_value = w_N_EN_fun(Rm_value(index),Rn_value(index),alt_value(index),lat_value(index)*D2R,ve_value(index),vn_value(index)); 385 | w_N_IE_value = w_N_IE_fun(lat_value(index)*D2R,we_value); 386 | w_L_IL_value = C_LN*(w_N_EN_value + w_N_IE_value); 387 | w_B_IL_value = C_LB_value'*w_L_IL_value; 388 | corriollis_effect_vector_in_B = C_LB_value'*C_LN*cross((w_N_EN_value + 2*w_N_IE_value),[ve_value(index);vn_value(index);vu_value(index)]); 389 | 390 | %--> Perform attitude quaternion mechanization 391 | a_dot_value = a_dot_fun(Rm_value(index),Rn_value(index),alt_value(index),b_value(index),c_value(index),d_value(index),gyro_x(index),gyro_y(index),gyro_z(index),gyro_sf_x_value(index),gyro_sf_y_value(index),gyro_sf_z_value(index),gyro_bias_x_value(index),gyro_bias_y_value(index),gyro_bias_z_value(index),0.0,0.0,0.0,lat_value(index)*D2R,ve_value(index),vn_value(index),we_value,wg_noise_value(index)); 392 | b_dot_value = b_dot_fun(Rm_value(index),Rn_value(index),alt_value(index),b_value(index),c_value(index),d_value(index),gyro_x(index),gyro_y(index),gyro_z(index),gyro_sf_x_value(index),gyro_sf_y_value(index),gyro_sf_z_value(index),gyro_bias_x_value(index),gyro_bias_y_value(index),gyro_bias_z_value(index),0.0,0.0,0.0,lat_value(index)*D2R,ve_value(index),vn_value(index),we_value,wg_noise_value(index)); 393 | c_dot_value = c_dot_fun(Rm_value(index),Rn_value(index),alt_value(index),b_value(index),c_value(index),d_value(index),gyro_x(index),gyro_y(index),gyro_z(index),gyro_sf_x_value(index),gyro_sf_y_value(index),gyro_sf_z_value(index),gyro_bias_x_value(index),gyro_bias_y_value(index),gyro_bias_z_value(index),0.0,0.0,0.0,lat_value(index)*D2R,ve_value(index),vn_value(index),we_value,wg_noise_value(index)); 394 | d_dot_value = d_dot_fun(Rm_value(index),Rn_value(index),alt_value(index),b_value(index),c_value(index),d_value(index),gyro_x(index),gyro_y(index),gyro_z(index),gyro_sf_x_value(index),gyro_sf_y_value(index),gyro_sf_z_value(index),gyro_bias_x_value(index),gyro_bias_y_value(index),gyro_bias_z_value(index),0.0,0.0,0.0,lat_value(index)*D2R,ve_value(index),vn_value(index),we_value,wg_noise_value(index)); 395 | 396 | if (~isreal([a_dot_value b_dot_value c_dot_value d_dot_value])) 397 | disp('imaginary quaternion rate');return; 398 | end 399 | 400 | %--> Advance quaternion 401 | a_value(index+1) = a_value(index) + a_dot_value*sampling_period_sec; 402 | b_value(index+1) = b_value(index) + b_dot_value*sampling_period_sec; 403 | c_value(index+1) = c_value(index) + c_dot_value*sampling_period_sec; 404 | d_value(index+1) = d_value(index) + d_dot_value*sampling_period_sec; 405 | 406 | %--> Normalize the quaternion 407 | normalized_quat = quatnormalize([a_value(index+1) b_value(index+1) c_value(index+1) d_value(index+1)]); 408 | a_value(index+1) = normalized_quat(1); 409 | b_value(index+1) = normalized_quat(2); 410 | c_value(index+1) = normalized_quat(3); 411 | d_value(index+1) = normalized_quat(4); 412 | 413 | %--> Calculate Euler angles from Quaternion 414 | [A, p, r] = quat2angle([a_value(index+1) b_value(index+1) c_value(index+1) d_value(index+1)],'ZYX'); 415 | Euler_pitch_value(index+1) = p; 416 | Euler_roll_value(index+1) = r; 417 | Euler_heading_value(index+1) = A; 418 | 419 | if (~isreal([Euler_roll_value(index+1) Euler_pitch_value(index+1) Euler_heading_value(index+1)])) 420 | disp('imaginary Euler angles');return; 421 | end 422 | 423 | %-->Reset quaternion in case -180/180 boundaries crossed 424 | quat_vector = angle2quat(Euler_heading_value(index+1),Euler_pitch_value(index+1),Euler_roll_value(index+1),'ZYX'); 425 | a_value(index+1) = quat_vector(1); 426 | b_value(index+1) = quat_vector(2); 427 | c_value(index+1) = quat_vector(3); 428 | d_value(index+1) = quat_vector(4); 429 | 430 | normalized_quat = quatnormalize([a_value(index+1) b_value(index+1) c_value(index+1) d_value(index+1)]); 431 | a_value(index+1) = normalized_quat(1); 432 | b_value(index+1) = normalized_quat(2); 433 | c_value(index+1) = normalized_quat(3); 434 | d_value(index+1) = normalized_quat(4); 435 | 436 | C_BL_value = angle2dcm(Euler_heading_value(index+1),Euler_pitch_value(index+1),Euler_roll_value(index+1),'ZYX'); 437 | C_LB_value = C_BL_value'; 438 | 439 | %--> Advance velocity 440 | V_N_dot_value = V_N_dot_fun(Rm_value(index),Rn_value(index),accel_x(index),accel_y(index),accel_z(index),... 441 | acc_sf_x_value(index),acc_sf_y_value(index),acc_sf_z_value(index),... 442 | acc_bias_x_value(index),acc_bias_y_value(index),acc_bias_z_value(index),0.0,0.0,0.0,... 443 | alt_value(index),b_value(index),c_value(index),d_value(index),lat_value(index)*D2R,ve_value(index),vn_value(index),vu_value(index),we_value,wg_noise_value(index)); 444 | 445 | if (~isreal(V_N_dot_value)) 446 | disp('imaginary velocity rate');return; 447 | end 448 | 449 | ve_value(index+1) = ve_value(index) + V_N_dot_value(1)*sampling_period_sec; 450 | vn_value(index+1) = vn_value(index) + V_N_dot_value(2)*sampling_period_sec; 451 | vu_value(index+1) = vu_value(index) + V_N_dot_value(3)*sampling_period_sec; 452 | 453 | %--> Advance position 454 | pos_quat_dot_value = pos_quat_dot_fun_EN(Rm_value(index),Rn_value(index),a_pos_rw_stdv_value(index), alt_value(index),... 455 | b_pos_value(index),b_pos_rw_stdv_value(index), c_pos_value(index),c_pos_rw_stdv_value(index), d_pos_value(index),d_pos_rw_stdv_value(index),... 456 | lat_value(index)*D2R,ve_value(index),vn_value(index), wg_noise_value(index)); 457 | 458 | if (~isreal(pos_quat_dot_value)) 459 | disp('imaginary position quaternion rate');return; 460 | end 461 | 462 | a_pos_value(index+1) = a_pos_value(index) + pos_quat_dot_value(1)*sampling_period_sec; 463 | b_pos_value(index+1) = b_pos_value(index) + pos_quat_dot_value(2)*sampling_period_sec; 464 | c_pos_value(index+1) = c_pos_value(index) + pos_quat_dot_value(3)*sampling_period_sec; 465 | d_pos_value(index+1) = d_pos_value(index) + pos_quat_dot_value(4)*sampling_period_sec; 466 | 467 | normalization_factor = sqrt(a_pos_value(index+1)^2 + b_pos_value(index+1)^2 + c_pos_value(index+1)^2 + d_pos_value(index+1)^2); 468 | a_pos_value(index+1) = a_pos_value(index+1)/normalization_factor; 469 | b_pos_value(index+1) = b_pos_value(index+1)/normalization_factor; 470 | c_pos_value(index+1) = c_pos_value(index+1)/normalization_factor; 471 | d_pos_value(index+1) = d_pos_value(index+1)/normalization_factor; 472 | 473 | %--> Calculate position DCM from position quaternion 474 | C_EN_value = C_EN_from_pos_quat_fun(b_pos_value(index+1),c_pos_value(index+1),d_pos_value(index+1)); 475 | 476 | if (~isreal(C_EN_value)) 477 | disp('imaginary position DCM');return; 478 | end 479 | 480 | %--> Calculate lat and lon from position DCM 481 | lon_from_C_EN_value = atan2(C_EN_value(1,3),C_EN_value(3,3)); 482 | lat_from_C_EN_value = atan2(C_EN_value(2,3),sqrt(C_EN_value(2,1)^2+C_EN_value(2,2)^2)); 483 | lat_value(index+1) = lat_from_C_EN_value*R2D; 484 | lon_value(index+1) = lon_from_C_EN_value*R2D; 485 | 486 | %--> Advance altitude 487 | alt_value(index+1) = alt_value(index) + vu_value(index)*sampling_period_sec; 488 | 489 | %--> Calculate new earth parameters and east, north position components 490 | Rn_value(index+1) = earth_a/sqrt(1-earth_e2*sin(lat_value(index+1)*D2R)*sin(lat_value(index+1)*D2R)); 491 | Rm_value(index+1) = earth_a*(1-earth_e2)/((1-earth_e2*sin(lat_value(index+1)*D2R)*sin(lat_value(index+1)*D2R))^(1.5)); 492 | EP_value(index+1) = (lon_value(index+1)-lon_value(1))*D2R*(Rn_value(index+1)+alt_value(index+1))*cos(lat_value(index+1)*D2R); 493 | NP_value(index+1) = (lat_value(index+1)-lat_value(1))*D2R*(Rm_value(index+1)+alt_value(index+1)); 494 | 495 | %--> Advance biases 496 | gyro_bias_x_value(index+1) = gyro_bias_x_value(index); 497 | gyro_bias_y_value(index+1) = gyro_bias_y_value(index); 498 | gyro_bias_z_value(index+1) = gyro_bias_z_value(index); 499 | acc_bias_x_value(index+1) = acc_bias_x_value(index); 500 | acc_bias_y_value(index+1) = acc_bias_y_value(index); 501 | acc_bias_z_value(index+1) = acc_bias_z_value(index); 502 | 503 | %--> Advance scale factors 504 | gyro_sf_x_value(index+1) = gyro_sf_x_value(index); 505 | gyro_sf_y_value(index+1) = gyro_sf_y_value(index); 506 | gyro_sf_z_value(index+1) = gyro_sf_z_value(index); 507 | acc_sf_x_value(index+1) = acc_sf_x_value(index); 508 | acc_sf_y_value(index+1) = acc_sf_y_value(index); 509 | acc_sf_z_value(index+1) = acc_sf_z_value(index); 510 | 511 | %--> Advance receiver clock bias and drift 512 | receiver_clk_bias_value(index+1) = receiver_clk_bias_value(index); 513 | receiver_clk_drift_value(index+1) = receiver_clk_drift_value(index); 514 | 515 | %--> Kalman Filter model matrices calculation 516 | F_value = F_fun(Rm_value(index),Rn_value(index),accel_x(index),accel_y(index),accel_z(index),... 517 | acc_sf_x_value(index),acc_sf_y_value(index),acc_sf_z_value(index),... 518 | acc_bias_x_value(index),acc_bias_y_value(index),acc_bias_z_value(index),... 519 | acc_rw_stdv_x_value(index),acc_rw_stdv_y_value(index),acc_rw_stdv_z_value(index),... 520 | acc_sf_x_time_cnst_value(index),acc_sf_y_time_cnst_value(index),acc_sf_z_time_cnst_value(index),... 521 | acc_bias_x_time_cnst_value(index),acc_bias_y_time_cnst_value(index),acc_bias_z_time_cnst_value(index),... 522 | alt_value(index),b_value(index),c_value(index),d_value(index),... 523 | gyro_x(index),gyro_y(index),gyro_z(index),... 524 | gyro_sf_x_value(index),gyro_sf_y_value(index),gyro_sf_z_value(index),... 525 | gyro_bias_x_value(index),gyro_bias_y_value(index),gyro_bias_z_value(index),... 526 | gyro_rw_stdv_x_value(index),gyro_rw_stdv_y_value(index),gyro_rw_stdv_z_value(index),... 527 | gyro_sf_x_time_cnst_value(index),gyro_sf_y_time_cnst_value(index),gyro_sf_z_time_cnst_value(index),... 528 | gyro_bias_x_time_cnst_value(index),gyro_bias_y_time_cnst_value(index),gyro_bias_z_time_cnst_value(index),... 529 | lat_value(index)*R2D,ve_value(index),vn_value(index),vu_value(index),we_value,wg_noise_value(index)); 530 | 531 | if (~isreal(F_value)) 532 | disp('imaginary transition matrix');return; 533 | end 534 | 535 | Phi = eye(size(F_value)) + F_value*sampling_period_sec; 536 | 537 | G_value = G_fun(acc_rw_stdv_x_value(index),acc_rw_stdv_y_value(index),acc_rw_stdv_z_value(index),... 538 | acc_sf_x_time_cnst_value(index),acc_sf_y_time_cnst_value(index),acc_sf_z_time_cnst_value(index),... 539 | acc_bias_x_time_cnst_value(index),acc_bias_z_time_cnst_value(index),acc_bias_y_time_cnst_value(index),... 540 | acc_sf_gauss_markov_stdv_x_value(index),acc_sf_gauss_markov_stdv_y_value(index),acc_sf_gauss_markov_stdv_z_value(index),... 541 | acc_bias_gauss_markov_stdv_x_value(index),acc_bias_gauss_markov_stdv_y_value(index),acc_bias_gauss_markov_stdv_z_value(index),.... 542 | alt_rw_stdv_value(index),... 543 | b_value(index), c_value(index),d_value(index),... 544 | gyro_rw_stdv_x_value(index),gyro_rw_stdv_y_value(index),gyro_rw_stdv_z_value(index),... 545 | gyro_sf_x_time_cnst_value(index),gyro_sf_y_time_cnst_value(index),gyro_sf_z_time_cnst_value(index),... 546 | gyro_bias_x_time_cnst_value(index),gyro_bias_y_time_cnst_value(index),gyro_bias_z_time_cnst_value(index),... 547 | gyro_sf_gauss_markov_stdv_x_value(index),gyro_sf_gauss_markov_stdv_y_value(index),gyro_sf_gauss_markov_stdv_z_value(index),... 548 | gyro_bias_gauss_markov_stdv_x_value(index),gyro_bias_gauss_markov_stdv_y_value(index),gyro_bias_gauss_markov_stdv_z_value(index),... 549 | receiver_clk_bias_stdv_value(index), receiver_clk_drift_stdv_value(index)); 550 | 551 | if (~isreal(G_value)) 552 | disp('imaginary noise shaping matrix');return; 553 | end 554 | 555 | Qd = sampling_period_sec^2*(G_value*G_value'); 556 | 557 | %--> Advance state error covariance matrix 558 | P = Phi*P*Phi' + Qd; 559 | 560 | if (~isreal(P)) 561 | disp('imaginary noise covariance matrix');return; 562 | end 563 | 564 | %--> Apply updates from obervations 565 | if (mod(index,sampling_freq) == 0) 566 | %--> Initialize error vector and matrices 567 | clear z H R; 568 | 569 | no_of_sats = ems_data.no_of_sat{((index)/sampling_freq)+1}; 570 | 571 | %--> Convert reciever position from Geodetic frame to ECEF frame 572 | x = (Rn_value(index+1) + alt_value(index+1))*cos(lat_from_C_EN_value)*cos(lon_from_C_EN_value); 573 | y = (Rn_value(index+1) + alt_value(index+1))*cos(lat_from_C_EN_value)*sin(lon_from_C_EN_value); 574 | z = ((Rn_value(index+1)*(1-earth_e2))+alt_value(index+1))*sin(lat_from_C_EN_value); 575 | 576 | %--> Set the innovation sequence 577 | deltaPx = (x.*ones(no_of_sats,1)) - (ems_data.sat_Px{((index)/sampling_freq)+1}); 578 | deltaPy = (y.*ones(no_of_sats,1)) - (ems_data.sat_Py{((index)/sampling_freq)+1}); 579 | deltaPz = (z.*ones(no_of_sats,1)) - (ems_data.sat_Pz{((index)/sampling_freq)+1}); 580 | 581 | range_from_INS = sqrt(deltaPx.^2+deltaPy.^2+deltaPz.^2); 582 | range_from_GNSS = cell2mat(ems_data.sim_sat_range(((index)/sampling_freq)+1)) + (ems_data.noiseInfo.clk_bias - receiver_clk_bias_value(index+1)).*ones(no_of_sats,1); 583 | 584 | z = range_from_GNSS - range_from_INS; 585 | 586 | %--> Set the measurment matrix jacobian 587 | H = zeros(no_of_sats, 23); 588 | 589 | for m=1:no_of_sats 590 | H(m,:) = H_row_range_fun(Rm_value(index+1),Rn_value(index+1),alt_value(index+1),... 591 | (lon_from_C_EN_value - ems_data.lon(1))*(Rn_value(index+1)+alt_value(index+1))*cos(lat_from_C_EN_value)+ems_data.east(1),... 592 | ems_data.east(1),ems_data.lat(1),ems_data.lon(1),(lat_from_C_EN_value - ems_data.lat(1))*(Rm_value(index+1)+alt_value(index+1)),... 593 | ems_data.north(1),ems_data.sat_Px{(index/sampling_freq)+1}(m),ems_data.sat_Py{(index/sampling_freq)+1}(m),ems_data.sat_Pz{(index/sampling_freq)+1}(m)); 594 | end 595 | 596 | %--> Adjust measurment noise matrix 597 | R = diag(ones(1*no_of_sats,1)); 598 | 599 | range_error_std = parameters(56); 600 | 601 | R(1:no_of_sats,1:no_of_sats) = R(1:no_of_sats,1:no_of_sats).*(range_error_std.^2); 602 | 603 | 604 | if (~isreal(P)) 605 | disp('imaginary updated error covariance matrix ');return; 606 | end 607 | 608 | %--> EKF update 609 | K = (P*H')/(H*P*H'+R); 610 | state_correction_vector = K*z; 611 | P = P - K*H*P; 612 | 613 | %--> Normalize P 614 | P = (P+P')/2; 615 | 616 | if (~isreal(P)) 617 | disp('imaginary error covariance matrix ');return; 618 | end 619 | 620 | %--> Correct states 621 | correct_states_TC; 622 | end 623 | 624 | 625 | gyro_rw_stdv_x_value(index+1) = gyro_rw_stdv_x_value(index); 626 | gyro_rw_stdv_y_value(index+1) = gyro_rw_stdv_y_value(index); 627 | gyro_rw_stdv_z_value(index+1) = gyro_rw_stdv_z_value(index); 628 | gyro_bias_gauss_markov_stdv_x_value (index+1) = gyro_bias_gauss_markov_stdv_x_value (index); 629 | gyro_bias_gauss_markov_stdv_y_value (index+1) = gyro_bias_gauss_markov_stdv_y_value (index); 630 | gyro_bias_gauss_markov_stdv_z_value (index+1) = gyro_bias_gauss_markov_stdv_z_value (index); 631 | gyro_bias_x_time_cnst_value(index+1) = gyro_bias_x_time_cnst_value(index); 632 | gyro_bias_y_time_cnst_value(index+1) = gyro_bias_y_time_cnst_value(index); 633 | gyro_bias_z_time_cnst_value(index+1) = gyro_bias_z_time_cnst_value(index); 634 | gyro_sf_gauss_markov_stdv_x_value (index+1) = gyro_sf_gauss_markov_stdv_x_value (index); 635 | gyro_sf_gauss_markov_stdv_y_value (index+1) = gyro_sf_gauss_markov_stdv_y_value (index); 636 | gyro_sf_gauss_markov_stdv_z_value (index+1) = gyro_sf_gauss_markov_stdv_z_value (index); 637 | gyro_sf_x_time_cnst_value(index+1) = gyro_sf_x_time_cnst_value(index); 638 | gyro_sf_y_time_cnst_value(index+1) = gyro_sf_y_time_cnst_value(index); 639 | gyro_sf_z_time_cnst_value(index+1) = gyro_sf_z_time_cnst_value(index); 640 | 641 | acc_rw_stdv_x_value(index+1) = acc_rw_stdv_x_value(index); 642 | acc_rw_stdv_y_value(index+1) = acc_rw_stdv_y_value(index); 643 | acc_rw_stdv_z_value(index+1) = acc_rw_stdv_z_value(index); 644 | acc_bias_gauss_markov_stdv_x_value (index+1) = acc_bias_gauss_markov_stdv_x_value (index); 645 | acc_bias_gauss_markov_stdv_y_value (index+1) = acc_bias_gauss_markov_stdv_y_value (index); 646 | acc_bias_gauss_markov_stdv_z_value (index+1) = acc_bias_gauss_markov_stdv_z_value (index); 647 | acc_bias_x_time_cnst_value(index+1) = acc_bias_x_time_cnst_value(index); 648 | acc_bias_y_time_cnst_value(index+1) = acc_bias_y_time_cnst_value(index); 649 | acc_bias_z_time_cnst_value(index+1) = acc_bias_z_time_cnst_value(index); 650 | acc_sf_gauss_markov_stdv_x_value (index+1) = acc_sf_gauss_markov_stdv_x_value (index); 651 | acc_sf_gauss_markov_stdv_y_value (index+1) = acc_sf_gauss_markov_stdv_y_value (index); 652 | acc_sf_gauss_markov_stdv_z_value (index+1) = acc_sf_gauss_markov_stdv_z_value (index); 653 | acc_sf_x_time_cnst_value(index+1) = acc_sf_x_time_cnst_value(index); 654 | acc_sf_y_time_cnst_value(index+1) = acc_sf_y_time_cnst_value(index); 655 | acc_sf_z_time_cnst_value(index+1) = acc_sf_z_time_cnst_value(index); 656 | 657 | a_pos_rw_stdv_value(index + 1) = a_pos_rw_stdv_value(index); 658 | b_pos_rw_stdv_value(index + 1) = b_pos_rw_stdv_value(index); 659 | c_pos_rw_stdv_value(index + 1) = c_pos_rw_stdv_value(index); 660 | d_pos_rw_stdv_value(index + 1) = d_pos_rw_stdv_value(index); 661 | alt_rw_stdv_value(index + 1) = alt_rw_stdv_value(index); 662 | 663 | wg_noise_value(index+1) = 0.0; 664 | 665 | g_value(index + 1) = gravity_fun(Rm_value(index),Rn_value(index),alt_value(index),lat_value(index)); 666 | 667 | %--> Disply results in 3D 668 | C_LB_value_plus_90 = C_LB_from_Euler_fun(Euler_roll_value(index),... 669 | -Euler_pitch_value(index),... 670 | pi/2 - Euler_heading_value(index)); 671 | 672 | updated_vert = C_LB_value_plus_90*initial_vert'; 673 | 674 | updated_vert = updated_vert'; 675 | 676 | for p = 1:length(updated_vert) 677 | updated_vert(p,:) = updated_vert(p,:) + [EP_value(index), NP_value(index), alt_value(index)]; 678 | end 679 | 680 | [ CubeXData , CubeYData , CubeZData ] = get_cube_axis_data(updated_vert); 681 | CubePoints = [updated_vert(:,1),updated_vert(:,2),updated_vert(:,3)]; 682 | 683 | %--> Create first figure after the 5th IMU sample 684 | if(index == 5) 685 | figure; 686 | hold on; 687 | plot3(ems_data.east,ems_data.north,ems_data.h, 'LineWidth', 1, 'color', [0.93 .69 .13]);grid on;xlabel('east');ylabel('north');zlabel('alt'); 688 | view(-1,50);title('3D Trajectory of Ground Truth, Noisy Updates, and EKF Solution'); 689 | h_noisy_updates = plot3(ems_data.east_noisy(1:20:index),ems_data.north_noisy(1:20:index),ems_data.h_noisy(1:20:index), '-*','MarkerSize', 5,'LineWidth', 0.5, 'color','k');grid on;xlabel('east');ylabel('north');zlabel('alt'); 690 | h_ekf_position = plot3(EP_value(1:index), NP_value(1:index), alt_value(1:index), 'color', 'r','LineWidth', 2);hold on;grid on; 691 | set(h_ekf_position,'YDataSource','NP_value(1:index)'); 692 | set(h_ekf_position,'XDataSource','EP_value(1:index)'); 693 | set(h_ekf_position,'ZDataSource','alt_value(1:index)'); 694 | set(h_noisy_updates,'YDataSource','ems_data.north_noisy(1:20:index)'); 695 | set(h_noisy_updates,'XDataSource','ems_data.east_noisy(1:20:index)'); 696 | set(h_noisy_updates,'ZDataSource','ems_data.h_noisy(1:20:index)'); 697 | h_vehicle = patch('Faces',faces,'Vertices',CubePoints,'FaceVertexCData',hsv(10),'FaceColor','flat'); 698 | set(h_vehicle,'XData',CubeXData,'YData',CubeYData,'ZData',CubeZData); 699 | set(gca,'FontSize',12); 700 | xlabel('East (m)');ylabel('North (m)');zlabel('Altitude(m)'); 701 | axis([min(ems_data.east) max(ems_data.east) min(ems_data.north) max(ems_data.north) -100 300]); 702 | legend({'Ground Truth','Noisy GPS','EKF Solution'}, 'FontSize', 12); 703 | else 704 | if enable_animation == 1 705 | if (mod(index,sampling_freq) == 0) 706 | refreshdata(h_ekf_position, 'caller'); 707 | refreshdata(h_noisy_updates, 'caller'); 708 | set(h_vehicle,'XData',CubeXData,'YData',CubeYData,'ZData',CubeZData); 709 | pause(0.05); 710 | end 711 | end 712 | end 713 | fprintf('processing epoch %d/%d\n',index,num_of_samples); 714 | end 715 | 716 | %--> Refresh figures 717 | refreshdata(h_ekf_position, 'caller'); 718 | refreshdata(h_noisy_updates, 'caller'); 719 | set(h_vehicle,'XData',CubeXData,'YData',CubeYData,'ZData',CubeZData); 720 | pause(0.05); 721 | 722 | %--> plot reference and EKF result 723 | figure; 724 | subplot (3,1,1); 725 | plot(ems_data.time,ems_data.roll.*R2D,'r', 'LineWidth',2); hold on; grid on; set(gca,'FontSize',12); 726 | plot(ems_data.time,Euler_roll_value.*R2D,'b', 'LineWidth',2); legend({'Reference','EKF Solution'}, 'FontSize', 12); xlabel('Time (s)');ylabel('Roll (deg)');title('Roll'); 727 | subplot (3,1,2); 728 | plot(ems_data.time,ems_data.pitch.*R2D,'r', 'LineWidth',2); hold on; grid on; set(gca,'FontSize',12); 729 | plot(ems_data.time,Euler_pitch_value.*R2D,'b', 'LineWidth',2); legend({'Reference','EKF Solution'}, 'FontSize', 12); xlabel('Time (s)');ylabel('Pitch (deg)');title('Pitch'); 730 | subplot (3,1,3); 731 | plot(ems_data.time,ems_data.heading.*R2D,'r', 'LineWidth',2); hold on; grid on; set(gca,'FontSize',12); 732 | plot(ems_data.time,Euler_heading_value.*R2D,'b', 'LineWidth',2);legend({'Reference','EKF Solution'}, 'FontSize', 12); xlabel('Time (s)');ylabel('Heading (deg)');title('Heading'); 733 | 734 | figure; 735 | subplot (3,1,1); 736 | plot(ems_data.time,ems_data.vel(:,1),'r', 'LineWidth',2); hold on; grid on; set(gca,'FontSize',12); 737 | plot(ems_data.time,vn_value,'b', 'LineWidth',2); legend({'Reference','EKF Solution'}, 'FontSize', 12); xlabel('Time (s)');ylabel('V_n (m/s)');title('V_n'); 738 | subplot (3,1,2); 739 | plot(ems_data.time,ems_data.vel(:,2),'r', 'LineWidth',2); hold on; grid on; set(gca,'FontSize',12); 740 | plot(ems_data.time,ve_value,'b', 'LineWidth',2); legend({'Reference','EKF Solution'}, 'FontSize', 12); xlabel('Time (s)');ylabel('V_e (m/s)');title('V_e'); 741 | subplot (3,1,3); 742 | plot(ems_data.time,ems_data.vel(:,3),'r', 'LineWidth',2); hold on; grid on; set(gca,'FontSize',12); 743 | plot(ems_data.time,-vu_value,'b', 'LineWidth',2); legend({'Reference','EKF Solution'}, 'FontSize', 12); xlabel('Time (s)');ylabel('V_d (m/s)');title('V_d'); 744 | 745 | figure; 746 | subplot (3,1,1); 747 | plot(ems_data.time,ems_data.north,'r', 'LineWidth',2); hold on; grid on; set(gca,'FontSize',12); 748 | plot(ems_data.time,NP_value,'b', 'LineWidth',2); legend({'Reference','EKF Solution'}, 'FontSize', 12); xlabel('Time (s)');ylabel('North Position (m)');title('North Position'); 749 | subplot (3,1,2); 750 | plot(ems_data.time,ems_data.east,'r', 'LineWidth',2); hold on; grid on; set(gca,'FontSize',12); 751 | plot(ems_data.time,EP_value,'b', 'LineWidth',2); legend({'Reference','EKF Solution'}, 'FontSize', 12); xlabel('Time (s)');ylabel('East Position (m)');title('East Position'); 752 | subplot (3,1,3); 753 | plot(ems_data.time,-ems_data.h,'r', 'LineWidth',2); hold on; grid on; set(gca,'FontSize',12); 754 | plot(ems_data.time,-alt_value,'b', 'LineWidth',2); legend({'Reference','EKF Solution'}, 'FontSize', 12); xlabel('Time (s)');ylabel('Down Position (m)');title('Down Position'); 755 | 756 | figure; 757 | plot(ems_data.time,receiver_clk_bias_value, 'b', 'LineWidth',2); hold on; grid on; set(gca,'FontSize',12); 758 | plot([ems_data.time(1), ems_data.time(end)], ems_data.noiseInfo.clk_bias*ones(1,2), '--b', 'LineWidth', 2); 759 | legend({'Estimated clock bias', sprintf('True clock bias(= %1.2f)', ems_data.noiseInfo.clk_bias)}, 'FontSize', 12);xlabel('Time (s)');ylabel('Bias (m)');title('Receiver Clock Bias'); 760 | 761 | figure; 762 | plot(ems_data.time, gyro_bias_x_value*R2D, 'r', 'LineWidth',2);grid on;hold on;set(gca,'FontSize',12); 763 | plot(ems_data.time, gyro_bias_y_value*R2D, 'g', 'LineWidth',2);hold on; 764 | plot(ems_data.time, gyro_bias_z_value*R2D, 'b', 'LineWidth',2);hold on; 765 | plot([ems_data.time(1), ems_data.time(end)], ems_data.noiseInfo.gyro_bias(1)*ones(1,2), '--r', 'LineWidth', 2);hold on; 766 | plot([ems_data.time(1), ems_data.time(end)], ems_data.noiseInfo.gyro_bias(2)*ones(1,2), '--g', 'LineWidth', 2);hold on; 767 | plot([ems_data.time(1), ems_data.time(end)], ems_data.noiseInfo.gyro_bias(3)*ones(1,2), '--b', 'LineWidth', 2);hold on; 768 | legend({'Estimated bias X','Estimated bias Y', 'Estimated bias Z', ... 769 | sprintf('True bias X (= %1.2f)', ems_data.noiseInfo.gyro_bias(1)),... 770 | sprintf('True bias Y (= %1.2f)', ems_data.noiseInfo.gyro_bias(2)),... 771 | sprintf('True bias Z (= %1.2f)', ems_data.noiseInfo.gyro_bias(3))}, 'FontSize', 12); xlabel('Time (s)');ylabel('Bias (deg/s)');title('Gyroscope Biases'); 772 | 773 | figure; 774 | plot(ems_data.time, acc_bias_x_value, 'r', 'LineWidth',2);grid on;hold on; set(gca,'FontSize',12); 775 | plot(ems_data.time, acc_bias_y_value, 'g', 'LineWidth',2);hold on; 776 | plot(ems_data.time, acc_bias_z_value, 'b', 'LineWidth',2);hold on; 777 | plot([ems_data.time(1), ems_data.time(end)], ems_data.noiseInfo.accel_bias(1)*ones(1,2), '--r', 'LineWidth', 2);hold on; 778 | plot([ems_data.time(1), ems_data.time(end)], ems_data.noiseInfo.accel_bias(2)*ones(1,2), '--g', 'LineWidth', 2);hold on; 779 | plot([ems_data.time(1), ems_data.time(end)], ems_data.noiseInfo.accel_bias(3)*ones(1,2), '--b', 'LineWidth', 2);hold on; 780 | legend({'Estimated bias X','Estimated bias Y', 'Estimated bias Z', ... 781 | sprintf('True bias X (= %1.2f)', ems_data.noiseInfo.accel_bias(1)),... 782 | sprintf('True bias Y (= %1.2f)', ems_data.noiseInfo.accel_bias(2)),... 783 | sprintf('True bias Z (= %1.2f)', ems_data.noiseInfo.accel_bias(3))}, 'FontSize', 12); xlabel('Time (s)');ylabel('Bias (m/s^2)');title('Accelerometer Biases'); 784 | 785 | 786 | %--> Plot errors 787 | v_east_ref_vector = ems_data.vel_N(:,1)'; 788 | v_north_ref_vector = ems_data.vel_N(:,2)'; 789 | v_up_ref_vector = ems_data.vel_N(:,3)'; 790 | p_east_ref_vector = ems_data.east'; 791 | p_north_ref_vector = ems_data.north'; 792 | alt_ref_vector = ems_data.h'; 793 | roll_ref_vector = ems_data.roll'; 794 | pitch_ref_vector = ems_data.pitch'; 795 | heading_ref_vector = ems_data.heading'; 796 | 797 | figure;title('Orientation Errors'); 798 | subplot(3,1,1); 799 | plot(ems_data.time, (roll_ref_vector' - Euler_roll_value)*R2D,'b', 'LineWidth',2);title('Roll Error'); grid on; xlabel('Time (s)');ylabel('Error (deg)'); set(gca,'FontSize',12); 800 | subplot(3,1,2); 801 | plot(ems_data.time, (pitch_ref_vector' - Euler_pitch_value)*R2D,'b', 'LineWidth',2);title('Pitch Error'); grid on; xlabel('Time (s)');ylabel('Error (deg)'); set(gca,'FontSize',12); 802 | subplot(3,1,3); 803 | plot(ems_data.time, (heading_ref_vector' - Euler_heading_value)*R2D,'b', 'LineWidth',2);title('Heading Error'); grid on; xlabel('Time (s)');ylabel('Error (deg)'); set(gca,'FontSize',12); 804 | 805 | figure;title('Velocity Errors'); 806 | subplot(3,1,2); 807 | plot(ems_data.time, v_east_ref_vector' - ve_value,'b', 'LineWidth',2);title('V_e Error'); grid on; xlabel('Time (s)');ylabel('Error (m/s)'); set(gca,'FontSize',12); 808 | subplot(3,1,1); 809 | plot(ems_data.time, v_north_ref_vector' - vn_value,'b', 'LineWidth',2);title('V_n Error'); grid on; xlabel('Time (s)');ylabel('Error (m/s)'); set(gca,'FontSize',12); 810 | subplot(3,1,3); 811 | plot(ems_data.time, -v_up_ref_vector' + vu_value,'b', 'LineWidth',2);title('V_d Error'); grid on; xlabel('Time (s)');ylabel('Error (m/s)'); set(gca,'FontSize',12); 812 | 813 | figure;title('Position Errors'); 814 | subplot(3,1,2); 815 | plot(ems_data.time, p_east_ref_vector' - EP_value,'b', 'LineWidth',2);title('East Position Error'); grid on; xlabel('Time (s)');ylabel('Error (m)'); set(gca,'FontSize',12); 816 | subplot(3,1,1); 817 | plot(ems_data.time, p_north_ref_vector' - NP_value,'b', 'LineWidth',2);title('North Position Error'); grid on; xlabel('Time (s)');ylabel('Error (m)'); set(gca,'FontSize',12); 818 | subplot(3,1,3); 819 | plot(ems_data.time, -alt_ref_vector' + alt_value,'b', 'LineWidth',2);title('Down Position Error'); grid on; xlabel('Time (s)');ylabel('Error (m)'); set(gca,'FontSize',12); 820 | 821 | %--> Print orientation, posiiton, and velocity errors 822 | pct = round(length(v_north_ref_vector)*.1); 823 | fprintf('\n\nError mean \n'); 824 | fprintf('V north error(m/s) = %.10f\n', sqrt(mean((v_north_ref_vector(pct:end)'-vn_value(pct:end)).^2))); 825 | fprintf('V east error(m/s) = %.10f\n', sqrt(mean((v_east_ref_vector(pct:end)'-ve_value(pct:end)).^2))); 826 | fprintf('V up error(m/s) = %.10f\n', sqrt(mean((v_up_ref_vector(pct:end)'-vu_value(pct:end)).^2))); 827 | fprintf('North error(m) = %.10f\n', sqrt(mean((p_north_ref_vector(pct:end)'-NP_value(pct:end)).^2))); 828 | fprintf('East error(m) = %.10f\n', sqrt(mean((p_east_ref_vector(pct:end)'-EP_value(pct:end)).^2))); 829 | fprintf('Alt error(m) = %.10f\n', sqrt(mean((alt_ref_vector(pct:end)'-alt_value(pct:end)).^2))); 830 | fprintf('Roll error(deg) = %.10f\n', sqrt(mean((roll_ref_vector(pct:end)'-Euler_roll_value(pct:end)).^2))*R2D); 831 | fprintf('Pitch error(deg) = %.10f\n', sqrt(mean((pitch_ref_vector(pct:end)'-Euler_pitch_value(pct:end)).^2))*R2D); 832 | fprintf('Heading error(deg) = %.10f\n', sqrt(mean((heading_ref_vector(pct:end)'-Euler_heading_value(pct:end)).^2))*R2D); 833 | 834 | fprintf('\n\nError std\n'); 835 | fprintf('V north error std(m/s) = %.10f\n', std(v_north_ref_vector(pct:end)'-vn_value(pct:end))); 836 | fprintf('V east error std(m/s) = %.10f\n', std(v_east_ref_vector(pct:end)'-ve_value(pct:end))); 837 | fprintf('V up error std(m/s) = %.10f\n', std(v_up_ref_vector(pct:end)'-vu_value(pct:end))); 838 | fprintf('North error std(m) = %.10f\n', std(p_north_ref_vector(pct:end)'-NP_value(pct:end))); 839 | fprintf('East error std(m) = %.10f\n', std(p_east_ref_vector(pct:end)'-EP_value(pct:end))); 840 | fprintf('Alt error std(m) = %.10f\n', std(alt_ref_vector(pct:end)'-alt_value(pct:end))); 841 | fprintf('Roll error std(deg) = %.10f\n', std(roll_ref_vector(pct:end)'-Euler_roll_value(pct:end))*R2D); 842 | fprintf('Pitch error std(deg) = %.10f\n', std(pitch_ref_vector(pct:end)'-Euler_pitch_value(pct:end))*R2D); 843 | fprintf('Heading error std(deg) = %.10f\n', std(heading_ref_vector(pct:end)'-Euler_heading_value(pct:end))*R2D); 844 | 845 | end 846 | -------------------------------------------------------------------------------- /ems_symbolic_engine_TC.m: -------------------------------------------------------------------------------- 1 | % Symbolic definistions of system dynamics, error models, and EKF models 2 | % 3 | % This is a basic INS/GNSS Tighly-Coupled system using EKF. This is only 4 | % a DEMO with basic updates (range measurements) are applied. More advanced 5 | % updates such as range rate measurements, nonholonomic constraints, 6 | % zero-speed, and adaptive EKF are NOT implemented in this DEMO. The purpose 7 | % of the DEMO is to demonstrate the basics of EKF in a basic INS/GNSS 8 | % Tightly-Coupled fusion scenario. 9 | % 10 | % For commercial use or embdded C/C++ versions, please contact mohamed.atia@carleton.ca. 11 | % 12 | % Copyright (C) 2019, Mohamed Atia, all rights reserved. 13 | % The software is given under GNU Lesser General Public License 14 | % This program is distributed in the hope that it will be useful, 15 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | % GNU Lesser General Public License for more details. 18 | % 19 | % You should have received a copy of the GNU Lesser General Public 20 | % License along with this program. If not, see 21 | % . 22 | 23 | 24 | %--> Symbols definitions 25 | reset(symengine); 26 | syms b c d; 27 | syms gyro_x gyro_y gyro_z; 28 | syms gyro_bias_x gyro_bias_y gyro_bias_z; 29 | syms gyro_bias_gauss_markov_stdv_x; 30 | syms gyro_bias_gauss_markov_stdv_y; 31 | syms gyro_bias_gauss_markov_stdv_z; 32 | syms w_el_x w_el_y w_el_z; 33 | syms wg_noise 34 | syms gyro_rw_stdv_x gyro_rw_stdv_y gyro_rw_stdv_z; 35 | syms gyro_bias_x_time_cnst gyro_bias_y_time_cnst gyro_bias_z_time_cnst; 36 | syms lat lon alt 37 | syms wander_angle 38 | syms w_N_IE we 39 | syms vn ve vu 40 | syms b_pos c_pos d_pos 41 | syms a_x a_y a_z 42 | syms acc_bias_x acc_bias_y acc_bias_z 43 | syms acc_bias_gauss_markov_stdv_x 44 | syms acc_bias_gauss_markov_stdv_y 45 | syms acc_bias_gauss_markov_stdv_z 46 | syms acc_rw_stdv_x acc_rw_stdv_y acc_rw_stdv_z 47 | syms acc_bias_x_time_cnst acc_bias_y_time_cnst acc_bias_z_time_cnst 48 | syms Rm Rn g lat0 lon0 49 | syms ve_dot vn_dot vu_dot 50 | syms Euler_pitch Euler_roll Euler_heading 51 | syms mag_x mag_y mag_z mag_bias_x mag_bias_y mag_bias_z 52 | syms mag_rw_stdv_x mag_rw_stdv_y mag_rw_stdv_z 53 | syms a_pos_initial_error_covariance 54 | syms b_pos_initial_error_covariance 55 | syms c_pos_initial_error_covariance 56 | syms d_pos_initial_error_covariance 57 | syms alt_initial_error_covariance 58 | syms ve_initial_error_covariance 59 | syms vn_initial_error_covariance 60 | syms vu_initial_error_covariance 61 | syms a_initial_error_covariance 62 | syms b_initial_error_covariance 63 | syms c_initial_error_covariance 64 | syms d_initial_error_covariance 65 | syms gyro_bias_x_initial_error_covariance 66 | syms gyro_bias_y_initial_error_covariance 67 | syms gyro_bias_z_initial_error_covariance 68 | syms acc_bias_x_initial_error_covariance 69 | syms acc_bias_y_initial_error_covariance 70 | syms acc_bias_z_initial_error_covariance 71 | syms a_pos_rw_stdv 72 | syms b_pos_rw_stdv 73 | syms c_pos_rw_stdv 74 | syms d_pos_rw_stdv 75 | syms alt_rw_stdv 76 | syms east_pos north_pos 77 | syms acc_sf_x acc_sf_y acc_sf_z 78 | syms gyro_sf_x gyro_sf_y gyro_sf_z 79 | syms gyro_sf_gauss_markov_stdv_x 80 | syms gyro_sf_gauss_markov_stdv_y 81 | syms gyro_sf_gauss_markov_stdv_z 82 | syms acc_sf_gauss_markov_stdv_x 83 | syms acc_sf_gauss_markov_stdv_y 84 | syms acc_sf_gauss_markov_stdv_z 85 | syms gyro_sf_x_time_cnst 86 | syms gyro_sf_y_time_cnst 87 | syms gyro_sf_z_time_cnst 88 | syms acc_sf_x_time_cnst 89 | syms acc_sf_y_time_cnst 90 | syms acc_sf_z_time_cnst 91 | syms receiver_clk_bias 92 | syms receiver_clk_bias_dot 93 | syms receiver_clk_bias_stdv 94 | syms receiver_clk_drift 95 | syms receiver_clk_drift_dot 96 | syms receiver_clk_drift_stdv 97 | syms lat0 lon0 north_pos0 east_pos0 98 | syms sat_x sat_y sat_z 99 | syms sat_vx sat_vy sat_vz 100 | syms x_pos y_pos z_pos 101 | syms x_vel y_vel z_vel 102 | syms line_of_sight 103 | syms sat_range 104 | 105 | 106 | %--> Degrees to Radians 107 | D2R = pi/180; 108 | R2D = 180/pi; 109 | 110 | %--> Earth ellipsoid shape parameters 111 | earth_a = 6378137; 112 | earth_f = 1/298.257223563; 113 | earth_b = earth_a*(1-earth_f); 114 | earth_e2 = 1-(earth_b^2)/(earth_a^2); 115 | 116 | %--> Definitions 117 | g_o = 9.780318 * ( 1 + 5.3024e-03.*sin(lat).^2 - 5.9e-06.*(sin(2*lat)).^2 ); 118 | g = (g_o ./ (1 + (alt ./ sqrt(Rn*Rm))).^2); 119 | g_N = [0; 0; -g]; 120 | V_N = [ve; vn; vu]; 121 | 122 | %--> raw measurements vectors 123 | acc_B = [((a_x-acc_bias_x)/(1+acc_sf_x))+acc_rw_stdv_x*wg_noise; 124 | ((a_y-acc_bias_y)/(1+acc_sf_y))+acc_rw_stdv_y*wg_noise; 125 | ((a_z-acc_bias_z)/(1+acc_sf_z))+acc_rw_stdv_z*wg_noise]; 126 | 127 | gyro_B = [((gyro_x-gyro_bias_x)/(1+gyro_sf_x))+gyro_rw_stdv_x*wg_noise; 128 | ((gyro_y-gyro_bias_y)/(1+gyro_sf_y))+gyro_rw_stdv_y*wg_noise; 129 | ((gyro_z-gyro_bias_z)/(1+gyro_sf_z))+gyro_rw_stdv_z*wg_noise]; 130 | 131 | mag_B = [mag_x-mag_bias_x+mag_rw_stdv_x*wg_noise; 132 | mag_y-mag_bias_y+mag_rw_stdv_y*wg_noise; 133 | mag_z-mag_bias_z+mag_rw_stdv_z*wg_noise]; 134 | 135 | 136 | %--> Frames transformation 137 | C_NL = [0 1 0; 138 | 1 0 0; 139 | 0 0 -1]; 140 | C_LN = C_NL'; 141 | 142 | %--> General transformation from N to E with wander angle 143 | D(1,1) = cos(lon)*cos(wander_angle) - sin(lon)*sin(lat)*sin(wander_angle); 144 | D(1,2) = -cos(lon)*sin(wander_angle) - sin(lon)*sin(lat)*cos(wander_angle); 145 | D(1,3) = sin(lon)*cos(lat); 146 | D(2,1) = cos(lat)*sin(wander_angle); 147 | D(2,2) = cos(lat)*cos(wander_angle); 148 | D(2,3) = sin(lat); 149 | D(3,1) = -sin(lon)*cos(wander_angle) - cos(lon)*sin(lat)*sin(wander_angle); 150 | D(3,2) = sin(lon)*sin(wander_angle)-cos(lon)*sin(lat)*cos(wander_angle); 151 | D(3,3) = cos(lon)*cos(lat); 152 | 153 | %--> This implementation follows "free wander angle (wander_angle = 0)" 154 | C_EN = subs(D,wander_angle,0); 155 | C_NE = C_EN.'; 156 | C_EL = C_EN*C_NL; 157 | 158 | %--> Quaternion Normalization Constraints 159 | a = sqrt(1- b^2 - c^2 - d^2); 160 | a_pos = sqrt(1 - b_pos^2 - c_pos^2 - d_pos^2); 161 | 162 | %--> DCM in terms of quaternion 163 | C_LB_from_quat = [ (a^2+b^2-c^2-d^2) 2*(b*c-a*d) 2*(b*d+a*c); 164 | 2*(b*c+a*d) (a^2-b^2+c^2-d^2) 2*(c*d-a*b); 165 | 2*(b*d-a*c) 2*(c*d + a*b) (a^2-b^2-c^2+d^2)]; 166 | 167 | %--> Attitude DCM in terms of Euler 168 | cp = cos(Euler_pitch); 169 | ch = cos(Euler_heading); 170 | sh = sin(Euler_heading); 171 | cr = cos(Euler_roll); 172 | sr = sin(Euler_roll); 173 | sp = sin(Euler_pitch); 174 | 175 | C_LB_from_Euler = [cp*ch -cr*sh + sr*sp*ch sr*sh + cr*sp*ch; 176 | cp*sh cr*ch + sr*sp*sh -sr*ch + cr*sp*sh; 177 | -sp sr*cp cr*cp]; 178 | 179 | %--> Transport rate 180 | w_N_EN = [-vn/(Rm+alt); ve/(Rn+alt); ve*tan(lat)/(Rn+alt)]; 181 | w_L_EN = C_LN*w_N_EN; 182 | w_L_EL = [ ve/(Rn+alt); -vn/(Rm+alt); -ve*tan(lat)/(Rn+alt)]; 183 | 184 | %--> Earth rate 185 | w_E_IE = [0; we; 0]; 186 | w_N_IE = C_NE*w_E_IE; 187 | w_L_IE = C_LN*w_N_IE; 188 | 189 | %--> Earth Rate + Transportation Rate compensation 190 | w_N_IN = w_N_IE + w_N_EN; 191 | w_L_IL = w_L_IE + w_L_EL; 192 | w_B_IL = C_LB_from_quat' * w_L_IL; 193 | 194 | w_il_x = w_B_IL(1); 195 | w_il_y = w_B_IL(2); 196 | w_il_z = w_B_IL(3); 197 | 198 | %--> Attitude quaternion system equations 199 | quat_vector = [a;b;c;d]; 200 | att_quat_matrix = [a -b -c -d; 201 | b a -d c; 202 | c d a -b; 203 | d -c b a]; 204 | gyro_quat_vector = [0;gyro_B]; 205 | 206 | %--> Rate compensation quaternion vector 207 | w_il_quat_matrix = [0 -w_il_x -w_il_y -w_il_z;... 208 | w_il_x 0 w_il_z -w_il_y;... 209 | w_il_y -w_il_z 0 w_il_x;... 210 | w_il_z w_il_y -w_il_x 0 ]; 211 | 212 | %--> Attitude system equations 213 | attitude_quat_dot = 0.5*att_quat_matrix*gyro_quat_vector - 0.5*w_il_quat_matrix*quat_vector; 214 | a_dot = attitude_quat_dot(1); 215 | b_dot = attitude_quat_dot(2); 216 | c_dot = attitude_quat_dot(3); 217 | d_dot = attitude_quat_dot(4); 218 | 219 | %--> Gyro bias system equations 220 | gyro_bias_x_dot = -gyro_bias_x/gyro_bias_x_time_cnst + gyro_bias_gauss_markov_stdv_x*sqrt(2/gyro_bias_x_time_cnst)*wg_noise; 221 | gyro_bias_y_dot = -gyro_bias_y/gyro_bias_y_time_cnst + gyro_bias_gauss_markov_stdv_y*sqrt(2/gyro_bias_y_time_cnst)*wg_noise; 222 | gyro_bias_z_dot = -gyro_bias_z/gyro_bias_z_time_cnst + gyro_bias_gauss_markov_stdv_z*sqrt(2/gyro_bias_z_time_cnst)*wg_noise; 223 | 224 | %--> Gyro scale factor system equations 225 | gyro_sf_x_dot = -gyro_sf_x/gyro_sf_x_time_cnst + gyro_sf_gauss_markov_stdv_x*sqrt(2/gyro_sf_x_time_cnst)*wg_noise; 226 | gyro_sf_y_dot = -gyro_sf_y/gyro_sf_y_time_cnst + gyro_sf_gauss_markov_stdv_y*sqrt(2/gyro_sf_y_time_cnst)*wg_noise; 227 | gyro_sf_z_dot = -gyro_sf_z/gyro_sf_z_time_cnst + gyro_sf_gauss_markov_stdv_z*sqrt(2/gyro_sf_z_time_cnst)*wg_noise; 228 | 229 | %--> Acc bias system equations 230 | acc_bias_x_dot = -acc_bias_x/acc_bias_x_time_cnst + acc_bias_gauss_markov_stdv_x*sqrt(2/acc_bias_x_time_cnst)*wg_noise; 231 | acc_bias_y_dot = -acc_bias_y/acc_bias_y_time_cnst + acc_bias_gauss_markov_stdv_y*sqrt(2/acc_bias_y_time_cnst)*wg_noise; 232 | acc_bias_z_dot = -acc_bias_z/acc_bias_z_time_cnst + acc_bias_gauss_markov_stdv_z*sqrt(2/acc_bias_z_time_cnst)*wg_noise; 233 | 234 | %--> Acc scale factor system equations 235 | acc_sf_x_dot = -acc_sf_x/acc_sf_x_time_cnst + acc_sf_gauss_markov_stdv_x*sqrt(2/acc_sf_x_time_cnst)*wg_noise; 236 | acc_sf_y_dot = -acc_sf_y/acc_sf_y_time_cnst + acc_sf_gauss_markov_stdv_y*sqrt(2/acc_sf_y_time_cnst)*wg_noise; 237 | acc_sf_z_dot = -acc_sf_z/acc_sf_z_time_cnst + acc_sf_gauss_markov_stdv_z*sqrt(2/acc_sf_z_time_cnst)*wg_noise; 238 | 239 | %--> Velocity equations 240 | V_N_dot = C_NL * C_LB_from_quat * acc_B + g_N - cross((w_N_EN + 2*w_N_IE),V_N); 241 | ve_dot = V_N_dot(1); 242 | vn_dot = V_N_dot(2); 243 | vu_dot = V_N_dot(3); 244 | 245 | %--> Position equations 246 | pos_quat_dot_EN = 0.5*[a_pos -b_pos -c_pos -d_pos; b_pos a_pos -d_pos c_pos;c_pos d_pos a_pos -b_pos; d_pos -c_pos b_pos a_pos]*[0;w_N_EN]... 247 | + [a_pos_rw_stdv;b_pos_rw_stdv;c_pos_rw_stdv;d_pos_rw_stdv]*wg_noise; 248 | pos_quat_dot_EL = 0.5*[a_pos -b_pos -c_pos -d_pos; b_pos a_pos -d_pos c_pos;c_pos d_pos a_pos -b_pos; d_pos -c_pos b_pos a_pos]*[0;w_L_EL]... 249 | + [a_pos_rw_stdv;b_pos_rw_stdv;c_pos_rw_stdv;d_pos_rw_stdv]*wg_noise; 250 | 251 | a_pos_dot = pos_quat_dot_EN(1); 252 | b_pos_dot = pos_quat_dot_EN(2); 253 | c_pos_dot = pos_quat_dot_EN(3); 254 | d_pos_dot = pos_quat_dot_EN(4); 255 | 256 | %--> Position quaternion in terms of lat/lon 257 | pos_quat_from_lat_lon = [cos(lon/2.0)*cos(-pi/4.0 - lat/2.0); 258 | -1*sin(lon/2.0)*sin(-pi/4.0 - lat/2.0); 259 | cos(lon/2.0)*sin(-pi/4.0 - lat/2.0); 260 | cos(-pi/4.0 - lat/2.0)*sin(lon/2.0);]; 261 | 262 | %--> Position DCM in terms of position quaternion 263 | C_EN_from_pos_quat = [(a_pos^2+b_pos^2-c_pos^2-d_pos^2) 2*(b_pos*c_pos-a_pos*d_pos) 2*(b_pos*d_pos+a_pos*c_pos); 264 | 2*(b_pos*c_pos+a_pos*d_pos) (a_pos^2-b_pos^2+c_pos^2-d_pos^2) 2*(c_pos*d_pos-a_pos*b_pos); 265 | 2*(b_pos*d_pos-a_pos*c_pos) 2*(c_pos*d_pos + a_pos*b_pos) (a_pos^2-b_pos^2-c_pos^2+d_pos^2)]; 266 | 267 | % OR (depends on which frame we are working on) 268 | C_EL_from_pos_quat = [(a_pos^2+b_pos^2-c_pos^2-d_pos^2) 2*(b_pos*c_pos-a_pos*d_pos) 2*(b_pos*d_pos+a_pos*c_pos); 269 | 2*(b_pos*c_pos+a_pos*d_pos) (a_pos^2-b_pos^2+c_pos^2-d_pos^2) 2*(c_pos*d_pos-a_pos*b_pos); 270 | 2*(b_pos*d_pos-a_pos*c_pos) 2*(c_pos*d_pos + a_pos*b_pos) (a_pos^2-b_pos^2-c_pos^2+d_pos^2)]; 271 | 272 | %--> Altituide System Equation: 273 | alt_dot = vu + alt_rw_stdv*wg_noise; 274 | 275 | %--> Alternatively, position can be modeled in east north components 276 | lat_dot = vn/(Rm+alt); 277 | lon_dot = ve/((Rn+alt)*cos(lat)); 278 | north_pos_dot = vn; %this is still an approximation since we ignored the efefct of transport rate rotation 279 | east_pos_dot = ve; %this is still an approximation since we ignored the efefct of transport rate rotation 280 | 281 | %--> East and North equations are as follows (still an approximation) 282 | east_dot = ve; 283 | north_dot = vn; 284 | 285 | %--> GPS receiver clock bias and drift equations 286 | receiver_clk_bias_dot = receiver_clk_drift + receiver_clk_bias_stdv*wg_noise; 287 | receiver_clk_drift_dot = receiver_clk_drift_stdv*wg_noise; 288 | 289 | %--> Geodetic and ENU position relation equations 290 | lat = (north_pos - north_pos0)/(Rm+alt) + lat0; 291 | lon = (east_pos - east_pos0)/((Rn+alt)*cos(lat)) + lon0; 292 | 293 | %--> Geodetic and ECEF position relation equations 294 | x_pos = (Rn+alt)*cos(lat)*cos(lon); 295 | y_pos = (Rn+alt)*cos(lat)*sin(lon); 296 | z_pos = (Rn*(1-earth_e2)+alt)*sin(lat); 297 | 298 | %--> Geodetic and ECEF velocity relation equations 299 | x_vel = -(Rn+alt)*sin(lat)*cos(lon)*lat_dot - (Rn+alt)*cos(lat)*sin(lon)*lon_dot + cos(lat)*cos(lon)*alt_dot; 300 | y_vel = -(Rn+alt)*sin(lat)*sin(lon)*lat_dot + (Rn+alt)*cos(lat)*cos(lon)*lon_dot + cos(lat)*sin(lon)*alt_dot; 301 | z_vel = (Rn*(1-earth_e2)+alt)*cos(lat)*lat_dot + sin(lat)*alt_dot; 302 | 303 | %--> Range equations 304 | sat_range = sqrt((x_pos - sat_x)^2 + (y_pos - sat_y)^2 + (z_pos - sat_z)^2) + receiver_clk_bias; 305 | 306 | 307 | %--> Symbols lists 308 | symbol_list = [east_pos;north_pos;alt;ve;vn;vu;b;c;d;gyro_bias_x;gyro_bias_y;gyro_bias_z;acc_bias_x;acc_bias_y;acc_bias_z;gyro_sf_x;gyro_sf_y;gyro_sf_z;acc_sf_x;acc_sf_y;acc_sf_z;receiver_clk_bias;receiver_clk_drift]; 309 | symbol_derivative_list = [east_pos_dot;north_pos_dot;alt_dot;ve_dot;vn_dot;vu_dot;b_dot;c_dot;d_dot;gyro_bias_x_dot;gyro_bias_y_dot;gyro_bias_z_dot;acc_bias_x_dot;acc_bias_y_dot;acc_bias_z_dot;gyro_sf_x_dot;gyro_sf_y_dot;gyro_sf_z_dot;acc_sf_x_dot;acc_sf_y_dot;acc_sf_z_dot;receiver_clk_bias_dot;receiver_clk_drift_dot]; 310 | 311 | %--> Jacobian of the nonlinear system transition matrix (F) 312 | for i = 1:length(symbol_list) 313 | for j = 1:length(symbol_list) 314 | F(i,j) = [diff(symbol_derivative_list(i), symbol_list(j))]; 315 | end 316 | end 317 | 318 | %--> Jacobian of the noise shaping vector (G) 319 | for j = 1:length(symbol_list) 320 | G(j,1) = diff(symbol_derivative_list(j), wg_noise); 321 | end 322 | 323 | 324 | %--> Jacobian of the nonlinear design matrix (H) 325 | for i = 1:length(symbol_list) 326 | H_row_range(1,i) = [diff(sat_range, symbol_list(i))]; 327 | end 328 | 329 | 330 | %--> Gravity measurement model 331 | quat_trans_matrix = [(a^2+b^2+c^2+d^2) 0 0 0; 332 | 0 (a^2+b^2-c^2-d^2) 2*(b*c-a*d) 2*(b*d+a*c); 333 | 0 2*(b*c+a*d) (a^2-b^2+c^2-d^2) 2*(c*d-a*b); 334 | 0 2*(b*d-a*c) 2*(c*d + a*b) (a^2-b^2-c^2+d^2)]; 335 | acc_quat_vector = [0;acc_B]; 336 | g_N_meas_vector = quat_trans_matrix*acc_quat_vector; 337 | for r = 1:length(g_N_meas_vector) 338 | for c = 1:length(symbol_list) 339 | g_N_meas_matrix(r,c) = [diff(g_N_meas_vector(r), symbol_list(c))]; 340 | end 341 | end 342 | 343 | %--> Magnetic measurement model 344 | mag_quat_vector = [0;mag_B]; 345 | m_N_meas_vector = quat_trans_matrix*mag_quat_vector; 346 | for r = 1:length(g_N_meas_vector) 347 | for c = 1:length(symbol_list) 348 | m_N_meas_matrix(r,c) = [diff(m_N_meas_vector(r), symbol_list(c))]; 349 | end 350 | end -------------------------------------------------------------------------------- /get_cube_axis_data.m: -------------------------------------------------------------------------------- 1 | % 3D Cube Drawing module 2 | % 3 | % This is a basic INS/GNSS Tighly-Coupled system using EKF. This is only 4 | % a DEMO with basic updates (range measurements) are applied. More advanced 5 | % updates such as range rate measurements, nonholonomic constraints, 6 | % zero-speed, and adaptive EKF are NOT implemented in this DEMO. The purpose 7 | % of the DEMO is to demonstrate the basics of EKF in a basic INS/GNSS 8 | % Tightly-Coupled fusion scenario. 9 | % 10 | % For commercial use or embdded C/C++ versions, please contact mohamed.atia@carleton.ca. 11 | % 12 | % Copyright (C) 2019, Mohamed Atia, all rights reserved. 13 | % The software is given under GNU Lesser General Public License 14 | % This program is distributed in the hope that it will be useful, 15 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | % GNU Lesser General Public License for more details. 18 | % 19 | % You should have received a copy of the GNU Lesser General Public 20 | % License along with this program. If not, see 21 | % . 22 | 23 | function [ CubeXData , CubeYData , CubeZData ] = get_cube_axis_data( ver ) 24 | CubeXData = [[ver(1,1);ver(2,1);ver(3,1);ver(4,1)] [ver(4,1);ver(3,1);ver(5,1);ver(6,1)]... 25 | [ver(6,1);ver(7,1);ver(8,1);ver(5,1)] [ver(1,1);ver(2,1);ver(8,1);ver(7,1)]... 26 | [ver(6,1);ver(7,1);ver(1,1);ver(4,1)] [ver(2,1);ver(3,1);ver(5,1);ver(8,1)]... 27 | [ver(1,1);ver(4,1);ver(10,1);ver(9,1)]... 28 | [ver(4,1);ver(10,1);ver(6,1);ver(6,1)]... 29 | [ver(6,1);ver(10,1);ver(9,1);ver(7,1)]... 30 | [ver(1,1);ver(9,1);ver(7,1);ver(7,1)]... 31 | %[ver(11,1);ver(12,1);ver(13,1);ver(13,1)]... 32 | %[ver(14,1);ver(15,1);ver(16,1);ver(16,1)]... 33 | %[ver(17,1);ver(18,1);ver(19,1);ver(19,1)]... 34 | ]; 35 | CubeYData = [[ver(1,2);ver(2,2);ver(3,2);ver(4,2)] [ver(4,2);ver(3,2);ver(5,2);ver(6,2)]... 36 | [ver(6,2);ver(7,2);ver(8,2);ver(5,2)] [ver(1,2);ver(2,2);ver(8,2);ver(7,2)]... 37 | [ver(6,2);ver(7,2);ver(1,2);ver(4,2)] [ver(2,2);ver(3,2);ver(5,2);ver(8,2)]... 38 | [ver(1,2);ver(4,2);ver(10,2);ver(9,2)]... 39 | [ver(4,2);ver(10,2);ver(6,2);ver(6,2)]... 40 | [ver(6,2);ver(10,2);ver(9,2);ver(7,2)]... 41 | [ver(1,2);ver(9,2);ver(7,2);ver(7,2)]... 42 | %[ver(11,2);ver(12,2);ver(13,2);ver(13,2)]... 43 | %[ver(14,2);ver(15,2);ver(16,2);ver(16,2)]... 44 | %[ver(17,2);ver(18,2);ver(19,2);ver(19,2)]... 45 | ]; 46 | CubeZData = [[ver(1,3);ver(2,3);ver(3,3);ver(4,3)] [ver(4,3);ver(3,3);ver(5,3);ver(6,3)]... 47 | [ver(6,3);ver(7,3);ver(8,3);ver(5,3)] [ver(1,3);ver(2,3);ver(8,3);ver(7,3)]... 48 | [ver(6,3);ver(7,3);ver(1,3);ver(4,3)] [ver(2,3);ver(3,3);ver(5,3);ver(8,3)]... 49 | [ver(1,3);ver(4,3);ver(10,3);ver(9,3)]... 50 | [ver(4,3);ver(10,3);ver(6,3);ver(6,3)]... 51 | [ver(6,3);ver(10,3);ver(9,3);ver(7,3)]... 52 | [ver(1,3);ver(9,3);ver(7,3);ver(7,3)]... 53 | %[ver(11,3);ver(12,3);ver(13,3);ver(13,3)]... 54 | %[ver(14,3);ver(15,3);ver(16,3);ver(16,3)]... 55 | %[ver(17,3);ver(18,3);ver(19,3);ver(19,3)]... 56 | ]; 57 | end --------------------------------------------------------------------------------