├── Dual_Freq_GPS.asv ├── Dual_Freq_GPS.m ├── FlightMech_Model.asv ├── FlightMech_Model.m ├── MainGPS.asv ├── MainGPS.m ├── README.md ├── RK4_autopilot.asv ├── RK4_autopilot.m ├── atmosphere.m ├── autopilotplant.mat ├── body_to_ecef.m ├── calc_sat_pos_ecef.asv ├── calc_sat_pos_ecef.m ├── chistart.m ├── collect_phase_data.asv ├── collect_phase_data.m ├── collect_phase_data2.m ├── compute_distance.m ├── ecef_to_latlong.asv ├── ecef_to_latlong.m ├── enu_to_ecef.m ├── estimate_user_position_cdgps2.asv ├── estimate_user_position_cdgps2.m ├── eval_DOP.m ├── eval_Thrust.m ├── eval_aerod_coeff.asv ├── eval_aerod_coeff.m ├── eval_delay_iono.m ├── eval_delay_tropo.m ├── eval_el_az.m ├── eval_gaccel_body.asv ├── eval_gaccel_ned.m ├── eval_integer_ambiguity.m ├── eval_ned_to_ecef.m ├── eval_pr_measurement.asv ├── eval_pr_measurement.m ├── eval_sat_clock_offset.m ├── eval_sbas_correction.m ├── eval_state_derivative.asv ├── eval_state_derivative.m ├── gaccel_e.m ├── gps_alamanac_data.m ├── inres_pos.m ├── inres_pos_data.m ├── integ_RK4.asv ├── integ_RK4.m ├── landing_trajectory.mat ├── latlong_to_ecef.m ├── ned_to_ecef.m ├── newton_raphson.m ├── rcvr_clk_model.m ├── select_optimum_sats.asv └── select_optimum_sats.m /Dual_Freq_GPS.asv: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 3 | % Dual frequency GPS Reciever Emulation Function % 4 | % Author: Saurav Agarwal % 5 | % Date: January 1, 2011 % 6 | % Dept. of Aerospace Engg., IIT Bombay, Mumbai, India % 7 | % All i/o units are specified in brackets % 8 | % Conventions: 9 | % 1. WGS-84 system used for geodetic model 10 | % 2. Geodetic Coordinates are used in the form lat(rad)/long(rad)/alt(m) 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | % References: 13 | % 1. Global Positioning System: Theory and Applications Vol I,Bradford W. Parksinson, 14 | % Outputs: 15 | % 1. user_pos_gps: reciver pos as calculated from gps measurements ECEF coordinates (m) 16 | % 2. optimum_sv_ids: list of satellites with lowest GDOP 17 | % 3. DOP: array containing GDOP,PDOP,HDOP,VDOP 18 | % 4. Velocity_Ecef: Velocity of reciever in ECEF frame (m) 19 | % 5. rcvr_clk_bias: clock bias (m) 20 | % Inputs: 21 | % true_user_pos_ecef,initial_user_pos_estimate,inres_pos_data,estimate_user_vel_ecef,true_user_vel_ecef 22 | % 1. initial_bias: initial clock bias (s) 23 | % 2. GPSMODE: all in view/4 satellite mode 24 | % 3. gps_sat: gps satellite ephemerides 25 | % 4. gps_time: (s) 26 | % 5. visible_sats_id:list of visible gps satellites 27 | % 6. true_user_pos_ecef: true ECEF pos (m) 28 | % 7. initial_user_pos_estimate: initial pos estimate in ECEF (m) 29 | % 8. inres_pos_data: indian GAGAN reference station position data 30 | % 9. estimate_user_vel_ecef: estimtae of user velocity (m/s) ECEF frame 31 | % 10. true_user_vel_ecef: true user velocity 32 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 33 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 34 | 35 | function [user_pos_gps,optimum_sv_ids,DOP,Velocity_Ecef,rcvr_clk_bias] = Dual_Freq_GPS(initial_bias,GPSMODE,gps_sat,gps_time,visible_sats_id,true_user_pos_ecef,initial_user_pos_estimate,inres_pos_data,estimate_user_vel_ecef,true_user_vel_ecef) 36 | 37 | c = 2.99792458e8;% speed of light (m/s) 38 | 39 | xu = initial_user_pos_estimate.x; 40 | yu = initial_user_pos_estimate.y; 41 | zu = initial_user_pos_estimate.z; 42 | 43 | user_ecef_pos_estimate = struct('x',xu,'y',yu,'z',zu); 44 | 45 | N = length(visible_sats_id); 46 | 47 | [optimum_sv_ids, GDOP,PDOP,HDOP,VDOP] = select_optimum_sats(gps_sat,gps_time,visible_sats_id,true_user_pos_ecef,initial_user_pos_estimate); 48 | DOP = [GDOP PDOP HDOP VDOP]; 49 | delta_x = 10; 50 | delta_y = 10; 51 | delta_z = 10; 52 | Cb = 0; 53 | rcvr_clk_bias = rcvr_clk_model(initial_bias,randn); 54 | %sbas_correction = 0;%eval_sbas_correction(gps_time,gps_sat,inres_pos_data,true_user_pos_ecef); % correction in seconds provided by aigmentation system 55 | % Initialise arrays for storage 56 | % sat_clk_drift = zeros(1,4); 57 | % sat_clk_rel_error =zeros(1,4); 58 | % xs = zeros(1,4); 59 | % ys = zeros(1,4); 60 | % zs = zeros(1,4); 61 | % pr_measured_L1 = zeros(1,4); 62 | % pr_measured_L2 = zeros(1,4); 63 | % pr_measured_L5 = zeros(1,4); 64 | % Vsat_ECEF = zeros(4,3); 65 | % pr_ionofree = zeros(1,4); 66 | % Gtrue = zeros(4,3); 67 | % L = zeros(1,4); 68 | % A = zeros(4,4); 69 | % AA = zeros(4,4); 70 | 71 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 72 | % All in view mode % 73 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 74 | if GPSMODE == 1 75 | 76 | for k = 1:N 77 | 78 | sv_id = visible_sats_id(k);%optimum_sv_ids(k); 79 | 80 | [sat_clk_drift(k),sat_clk_rel_error(k)] = eval_sat_clock_offset(gps_sat,sv_id,gps_time); % satellite clock offset in seconds (clock drift + relativistic error) 81 | 82 | [xs(k),ys(k),zs(k),Vsat_ECEF(k,:)] = calc_sat_pos_ecef(gps_sat,gps_time,sv_id); % The position of satellite based on ephemeris data and the time embedded in message 83 | 84 | computed_sat_pos_ecef(k) = struct('x',xs(k),'y',ys(k),'z',zs(k)); 85 | 86 | [pr_measured_L1(k), pr_measured_L2(k), pr_measured_L5] = eval_pr_measurement(gps_sat,sv_id,gps_time,true_user_pos_ecef, rcvr_clk_bias,computed_sat_pos_ecef(k)); 87 | 88 | pr_ionofree(k) = pr_measured_L1(k)*2.546 - pr_measured_L2(k)*1.546 + c*(sat_clk_drift(k)+sat_clk_rel_error(k)); % make it ionofree from page 166 mishra and enge and correct it for satellite clock error 89 | 90 | dtrue = compute_distance(computed_sat_pos_ecef(k),true_user_pos_ecef); 91 | 92 | Gtrue(k,:) = [(-(xs(k) - true_user_pos_ecef.x)/dtrue) (-(ys(k) - true_user_pos_ecef.y)/dtrue) (-(zs(k) - true_user_pos_ecef.z)/dtrue)]; 93 | end; 94 | 95 | while abs(delta_x)> 1e-3 && abs(delta_y)> 1e-3 && abs(delta_z)> 1e-3 96 | 97 | for k = 1:N 98 | 99 | d = compute_distance(computed_sat_pos_ecef(k),user_ecef_pos_estimate); 100 | 101 | pr_estimated = d + Cb ; % what the receiver thinks is true pseduo range 102 | 103 | L(k) = pr_ionofree(k) - pr_estimated ; %delta rho 104 | 105 | A(k,:) = [(-(xs(k) - xu)/d) (-(ys(k) - yu)/d) (-(zs(k) - zu)/d) 1]; 106 | 107 | end; 108 | 109 | error = (A'*A)^-1*A'*L'; 110 | 111 | Cb = Cb + error(4); 112 | 113 | delta_x = error(1); 114 | 115 | delta_y = error(2); 116 | 117 | delta_z = error(3); 118 | 119 | xu = xu + delta_x ; 120 | 121 | yu = yu+ delta_y ; 122 | 123 | zu = zu + delta_z ; 124 | 125 | user_ecef_pos_estimate = struct('x',xu,'y',yu,'z',zu); 126 | end 127 | end 128 | 129 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 130 | % Optimal satellite geometry mode % 131 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 132 | if GPSMODE == 2 133 | 134 | for k = 1:4 135 | 136 | sv_id = optimum_sv_ids(k);%optimum_sv_ids(k); 137 | 138 | [sat_clk_drift(k),sat_clk_rel_error(k)] = eval_sat_clock_offset(gps_sat,sv_id,gps_time); % satellite clock offset in seconds (clock drift + relativistic error) 139 | 140 | [xs(k),ys(k),zs(k),Vsat_ECEF(k,:)] = calc_sat_pos_ecef(gps_sat,gps_time,sv_id); % The position of satellite based on ephemeris data and the time embedded in message 141 | 142 | computed_sat_pos_ecef(k) = struct('x',xs(k),'y',ys(k),'z',zs(k)); 143 | 144 | [pr_measured_L1(k), pr_measured_L2(k), pr_measured_L5] = eval_pr_measurement(gps_sat,sv_id,gps_time,true_user_pos_ecef, rcvr_clk_bias,computed_sat_pos_ecef(k)); 145 | 146 | pr_ionofree(k) = pr_measured_L1(k)*2.546 - pr_measured_L2(k)*1.546 + c*(sat_clk_drift(k)+sat_clk_rel_error(k)); % make it ionofree (from page 166 mishra and enge) and correct for satellite clock error 147 | 148 | dtrue = compute_distance(computed_sat_pos_ecef(k),true_user_pos_ecef); 149 | 150 | Gtrue(k,:) = [(-(xs(k) - true_user_pos_ecef.x)/dtrue) (-(ys(k) - true_user_pos_ecef.y)/dtrue) (-(zs(k) - true_user_pos_ecef.z)/dtrue)]; 151 | end; 152 | 153 | while abs(delta_x)> 1e-3 && abs(delta_y)> 1e-3 && abs(delta_z)> 1e-3 154 | 155 | for k = 1:4 156 | 157 | d = compute_distance(computed_sat_pos_ecef(k),user_ecef_pos_estimate); % what the receiver thinks is true pseduo range 158 | 159 | pr_estimated = d + Cb; 160 | 161 | L(k) = pr_ionofree(k) - pr_estimated ; % delta rho 162 | 163 | A(k,:) = [(-(xs(k) - xu)/d) (-(ys(k) - yu)/d) (-(zs(k) - zu)/d) 1]; 164 | 165 | 166 | end; 167 | 168 | error = A^-1*L'; 169 | 170 | Cb = Cb + error(4); 171 | 172 | delta_x = error(1); 173 | 174 | delta_y = error(2); 175 | 176 | delta_z = error(3); 177 | 178 | xu = xu + delta_x ; 179 | 180 | yu = yu+ delta_y ; 181 | 182 | zu = zu + delta_z ; 183 | 184 | user_ecef_pos_estimate = struct('x',xu,'y',yu,'z',zu); 185 | end 186 | end 187 | user_pos_gps = user_ecef_pos_estimate; 188 | 189 | Velocity_Ecef = calc_user_velocity_ecef(A,Gtrue,Vsat_ECEF,estimate_user_vel_ecef,true_user_vel_ecef); 190 | 191 | end 192 | 193 | % Velocity estimation using doppler shift 194 | % Page 411 Parkinson Vol.I 195 | function [Velocity_ECEF] = calc_user_velocity_ecef(A,Gtrue,Vsat_ECEF,estimate_user_vel_ecef,true_user_vel_ecef); 196 | 197 | c = 2.99792458e8;% speed of light (m/s) 198 | delta_vx = 10; 199 | delta_vy = 10; 200 | delta_vz = 10; 201 | f = c*1e-6/(365*24*3600); % clock bias in m/s 202 | N = length(A); 203 | noise = randn; 204 | clkdrift_estimate = 0; 205 | while abs(delta_vx)> 1e-3 && abs(delta_vy)> 1e-3 && abs(delta_vz)> 1e-3 206 | 207 | for i = 1:N 208 | 209 | rho_dot(i) = dot((Vsat_ECEF(i)'-true_user_vel_ecef'),Gtrue(i,1:3)') + f + noise; % Measured pseudorange rate 210 | 211 | rho_cap_dot(i) = dot((Vsat_ECEF(i)'- estimate_user_vel_ecef'),A(i,1:3)') + clkdrift_estimate ; % estimated pseudo range rate 212 | 213 | delta_rho_dot(i) = rho_cap_dot(i) - rho_dot(i); 214 | 215 | 216 | end 217 | 218 | error = (A'*A)^-1*A'*delta_rho_dot'; % least squares 219 | 220 | clkdrift_estimate = clkdrift_estimate + error(4); 221 | 222 | delta_vx = error(1); 223 | 224 | delta_vy = error(2); 225 | 226 | delta_vz = error(3); 227 | 228 | estimate_user_vel_ecef = estimate_user_vel_ecef + [error(1) error(2) error(3)]; 229 | 230 | end; 231 | 232 | Velocity_ECEF = estimate_user_vel_ecef; % Estimated user velocity in ECEF frame 233 | end 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | -------------------------------------------------------------------------------- /Dual_Freq_GPS.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 3 | % Dual frequency GPS Reciever Emulation Function % 4 | % Author: Saurav Agarwal 5 | % Email: saurav6@gmail.com 6 | % Date: January 1, 2011 7 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 8 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 9 | % All i/o units are specified in brackets % 10 | % Conventions: 11 | % 1. WGS-84 system used for geodetic model 12 | % 2. Geodetic Coordinates are used in the form lat(rad)/long(rad)/alt(m) 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | % References: 15 | % 1. Global Positioning System: Theory and Applications Vol I,Bradford W. Parksinson, 16 | % Outputs: 17 | % 1. user_pos_gps: reciver pos as calculated from gps measurements ECEF coordinates (m) 18 | % 2. optimum_sv_ids: list of satellites with lowest GDOP 19 | % 3. DOP: array containing GDOP,PDOP,HDOP,VDOP 20 | % 4. Velocity_Ecef: Velocity of reciever in ECEF frame (m) 21 | % 5. rcvr_clk_bias: clock bias (m) 22 | % Inputs: 23 | % true_user_pos_ecef,initial_user_pos_estimate,inres_pos_data,estimate_user_vel_ecef,true_user_vel_ecef 24 | % 1. initial_bias: initial clock bias (s) 25 | % 2. GPSMODE: all in view/4 satellite mode 26 | % 3. gps_sat: gps satellite ephemerides 27 | % 4. gps_time: (s) 28 | % 5. visible_sats_id:list of visible gps satellites 29 | % 6. true_user_pos_ecef: true ECEF pos (m) 30 | % 7. initial_user_pos_estimate: initial pos estimate in ECEF (m) 31 | % 8. inres_pos_data: indian GAGAN reference station position data 32 | % 9. estimate_user_vel_ecef: estimtae of user velocity (m/s) ECEF frame 33 | % 10. true_user_vel_ecef: true user velocity (m/s) ECEF frame 34 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 35 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 36 | 37 | function [user_pos_gps,optimum_sv_ids,DOP,Velocity_Ecef,rcvr_clk_bias] = Dual_Freq_GPS(initial_bias,GPSMODE,gps_sat,gps_time,visible_sats_id,true_user_pos_ecef,initial_user_pos_estimate,inres_pos_data,estimate_user_vel_ecef,true_user_vel_ecef) 38 | 39 | c = 2.99792458e8;% speed of light (m/s) 40 | 41 | xu = initial_user_pos_estimate.x; 42 | yu = initial_user_pos_estimate.y; 43 | zu = initial_user_pos_estimate.z; 44 | 45 | user_ecef_pos_estimate = struct('x',xu,'y',yu,'z',zu); 46 | 47 | N = length(visible_sats_id); 48 | 49 | [optimum_sv_ids, GDOP,PDOP,HDOP,VDOP] = select_optimum_sats(gps_sat,gps_time,visible_sats_id,true_user_pos_ecef,initial_user_pos_estimate); 50 | DOP = [GDOP PDOP HDOP VDOP]; 51 | delta_x = 10; 52 | delta_y = 10; 53 | delta_z = 10; 54 | Cb = 0; 55 | rcvr_clk_bias = rcvr_clk_model(initial_bias,randn); 56 | %sbas_correction = 0;%eval_sbas_correction(gps_time,gps_sat,inres_pos_data,true_user_pos_ecef); % correction in seconds provided by aigmentation system 57 | % Initialise arrays for storage 58 | % sat_clk_drift = zeros(1,4); 59 | % sat_clk_rel_error =zeros(1,4); 60 | % xs = zeros(1,4); 61 | % ys = zeros(1,4); 62 | % zs = zeros(1,4); 63 | % pr_measured_L1 = zeros(1,4); 64 | % pr_measured_L2 = zeros(1,4); 65 | % pr_measured_L5 = zeros(1,4); 66 | % Vsat_ECEF = zeros(4,3); 67 | % pr_ionofree = zeros(1,4); 68 | % Gtrue = zeros(4,3); 69 | % L = zeros(1,4); 70 | % A = zeros(4,4); 71 | % AA = zeros(4,4); 72 | 73 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 74 | % All in view mode % 75 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 76 | if GPSMODE == 1 77 | 78 | for k = 1:N 79 | 80 | sv_id = visible_sats_id(k);%optimum_sv_ids(k); 81 | 82 | [sat_clk_drift(k),sat_clk_rel_error(k)] = eval_sat_clock_offset(gps_sat,sv_id,gps_time); % satellite clock offset in seconds (clock drift + relativistic error) 83 | 84 | [xs(k),ys(k),zs(k),Vsat_ECEF(k,:)] = calc_sat_pos_ecef(gps_sat,gps_time,sv_id); % The position of satellite based on ephemeris data and the time embedded in message 85 | 86 | computed_sat_pos_ecef(k) = struct('x',xs(k),'y',ys(k),'z',zs(k)); 87 | 88 | [pr_measured_L1(k), pr_measured_L2(k), pr_measured_L5] = eval_pr_measurement(gps_sat,sv_id,gps_time,true_user_pos_ecef, rcvr_clk_bias,computed_sat_pos_ecef(k)); 89 | 90 | pr_ionofree(k) = pr_measured_L1(k)*2.546 - pr_measured_L2(k)*1.546 + c*(sat_clk_drift(k)+sat_clk_rel_error(k)); % make it ionofree from page 166 mishra and enge and correct it for satellite clock error 91 | 92 | dtrue = compute_distance(computed_sat_pos_ecef(k),true_user_pos_ecef); 93 | 94 | Gtrue(k,:) = [(-(xs(k) - true_user_pos_ecef.x)/dtrue) (-(ys(k) - true_user_pos_ecef.y)/dtrue) (-(zs(k) - true_user_pos_ecef.z)/dtrue)]; 95 | end; 96 | 97 | while abs(delta_x)> 1e-3 && abs(delta_y)> 1e-3 && abs(delta_z)> 1e-3 98 | 99 | for k = 1:N 100 | 101 | d = compute_distance(computed_sat_pos_ecef(k),user_ecef_pos_estimate); 102 | 103 | pr_estimated = d + Cb ; % what the receiver thinks is true pseduo range 104 | 105 | L(k) = pr_ionofree(k) - pr_estimated ; %delta rho 106 | 107 | A(k,:) = [(-(xs(k) - xu)/d) (-(ys(k) - yu)/d) (-(zs(k) - zu)/d) 1]; 108 | 109 | end; 110 | 111 | error = (A'*A)^-1*A'*L'; 112 | 113 | Cb = Cb + error(4); 114 | 115 | delta_x = error(1); 116 | 117 | delta_y = error(2); 118 | 119 | delta_z = error(3); 120 | 121 | xu = xu + delta_x ; 122 | 123 | yu = yu+ delta_y ; 124 | 125 | zu = zu + delta_z ; 126 | 127 | user_ecef_pos_estimate = struct('x',xu,'y',yu,'z',zu); 128 | end 129 | end 130 | 131 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 132 | % Optimal satellite geometry mode % 133 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 134 | if GPSMODE == 2 135 | 136 | for k = 1:4 137 | 138 | sv_id = optimum_sv_ids(k);%optimum_sv_ids(k); 139 | 140 | [sat_clk_drift(k),sat_clk_rel_error(k)] = eval_sat_clock_offset(gps_sat,sv_id,gps_time); % satellite clock offset in seconds (clock drift + relativistic error) 141 | 142 | [xs(k),ys(k),zs(k),Vsat_ECEF(k,:)] = calc_sat_pos_ecef(gps_sat,gps_time,sv_id); % The position of satellite based on ephemeris data and the time embedded in message 143 | 144 | computed_sat_pos_ecef(k) = struct('x',xs(k),'y',ys(k),'z',zs(k)); 145 | 146 | [pr_measured_L1(k), pr_measured_L2(k), pr_measured_L5] = eval_pr_measurement(gps_sat,sv_id,gps_time,true_user_pos_ecef, rcvr_clk_bias,computed_sat_pos_ecef(k)); 147 | 148 | pr_ionofree(k) = pr_measured_L1(k)*2.546 - pr_measured_L2(k)*1.546 + c*(sat_clk_drift(k)+sat_clk_rel_error(k)); % make it ionofree (from page 166 mishra and enge) and correct for satellite clock error 149 | 150 | dtrue = compute_distance(computed_sat_pos_ecef(k),true_user_pos_ecef); 151 | 152 | Gtrue(k,:) = [(-(xs(k) - true_user_pos_ecef.x)/dtrue) (-(ys(k) - true_user_pos_ecef.y)/dtrue) (-(zs(k) - true_user_pos_ecef.z)/dtrue)]; 153 | end; 154 | 155 | while abs(delta_x)> 1e-3 && abs(delta_y)> 1e-3 && abs(delta_z)> 1e-3 156 | 157 | for k = 1:4 158 | 159 | d = compute_distance(computed_sat_pos_ecef(k),user_ecef_pos_estimate); % what the receiver thinks is true pseduo range 160 | 161 | pr_estimated = d + Cb; 162 | 163 | L(k) = pr_ionofree(k) - pr_estimated ; % delta rho 164 | 165 | A(k,:) = [(-(xs(k) - xu)/d) (-(ys(k) - yu)/d) (-(zs(k) - zu)/d) 1]; 166 | 167 | 168 | end; 169 | 170 | error = A^-1*L'; 171 | 172 | Cb = Cb + error(4); 173 | 174 | delta_x = error(1); 175 | 176 | delta_y = error(2); 177 | 178 | delta_z = error(3); 179 | 180 | xu = xu + delta_x ; 181 | 182 | yu = yu+ delta_y ; 183 | 184 | zu = zu + delta_z ; 185 | 186 | user_ecef_pos_estimate = struct('x',xu,'y',yu,'z',zu); 187 | end 188 | end 189 | user_pos_gps = user_ecef_pos_estimate; 190 | 191 | Velocity_Ecef = calc_user_velocity_ecef(A,Gtrue,Vsat_ECEF,estimate_user_vel_ecef,true_user_vel_ecef); 192 | 193 | end 194 | 195 | % Velocity estimation using doppler shift 196 | % Page 411 Parkinson Vol.I 197 | function [Velocity_ECEF] = calc_user_velocity_ecef(A,Gtrue,Vsat_ECEF,estimate_user_vel_ecef,true_user_vel_ecef); 198 | 199 | c = 2.99792458e8;% speed of light (m/s) 200 | delta_vx = 10; 201 | delta_vy = 10; 202 | delta_vz = 10; 203 | f = c*1e-6/(365*24*3600); % clock bias in m/s 204 | N = length(A); 205 | noise = randn; 206 | clkdrift_estimate = 0; 207 | while abs(delta_vx)> 1e-3 && abs(delta_vy)> 1e-3 && abs(delta_vz)> 1e-3 208 | 209 | for i = 1:N 210 | 211 | rho_dot(i) = dot((Vsat_ECEF(i)'-true_user_vel_ecef'),Gtrue(i,1:3)') + f + noise; % Measured pseudorange rate 212 | 213 | rho_cap_dot(i) = dot((Vsat_ECEF(i)'- estimate_user_vel_ecef'),A(i,1:3)') + clkdrift_estimate ; % estimated pseudo range rate 214 | 215 | delta_rho_dot(i) = rho_cap_dot(i) - rho_dot(i); 216 | 217 | 218 | end 219 | 220 | error = (A'*A)^-1*A'*delta_rho_dot'; % least squares 221 | 222 | clkdrift_estimate = clkdrift_estimate + error(4); 223 | 224 | delta_vx = error(1); 225 | 226 | delta_vy = error(2); 227 | 228 | delta_vz = error(3); 229 | 230 | estimate_user_vel_ecef = estimate_user_vel_ecef + [error(1) error(2) error(3)]; 231 | 232 | end; 233 | 234 | Velocity_ECEF = estimate_user_vel_ecef; % Estimated user velocity in ECEF frame 235 | end 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | -------------------------------------------------------------------------------- /FlightMech_Model.asv: -------------------------------------------------------------------------------- 1 | function [pos_ac_ecef, pos_ac_geodetic,Acceleration,Velocity_ecef,AngularVelocity,Attitude,X] = FlightMech_Model(dt,X,dth,de,da,dr) 2 | 3 | if (de > 20) 4 | de = 20; 5 | elseif (de<-20) 6 | de = -20; 7 | end 8 | if (da > 20) 9 | da = 20; 10 | elseif (da<-20) 11 | da = -20; 12 | end 13 | if (dr > 20) 14 | dr = 20; 15 | elseif (dr<-20) 16 | dr = -20; 17 | end 18 | 19 | % Calculating new state by integrating 20 | [X,DX] = integ_RK4(dt,X,dth,de,da,dr); 21 | 22 | % Acceleration 23 | Acceleration = DX(1:3); 24 | 25 | % Euler angles array 26 | Attitude = X(4:6); 27 | 28 | % Angular velocity 29 | AngularVelocity = X(7:9); 30 | 31 | % ECEF position 32 | pos_ac_ecef = struct('x',X(10),'y',X(11),'z',X(12)); 33 | 34 | % Geodetic Position 35 | pos_ac_geodetic = ecef_to_latlong(pos_ac_ecef); 36 | 37 | % Velocity in ECEF Coordinates 38 | Velocity_ecef = DX(10:12); 39 | 40 | end 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /FlightMech_Model.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Pioneer UAV Flight Mechanics Model 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | function [pos_ac_ecef, pos_ac_geodetic,Acceleration_body,AngV,X] = FlightMech_Model(dt,X,dth,de,da,dr) 9 | 10 | if (de > 20) 11 | de = 20; 12 | elseif (de<-20) 13 | de = -20; 14 | end 15 | if (da > 20) 16 | da = 20; 17 | elseif (da<-20) 18 | da = -20; 19 | end 20 | if (dr > 20) 21 | dr = 20; 22 | elseif (dr<-20) 23 | dr = -20; 24 | end 25 | 26 | % Calculating new state by integrating 27 | [X,DX,Acceleration_body,AngV] = integ_RK4(dt,X,dth,de,da,dr); 28 | 29 | % Geodetic Position 30 | pos_ac_geodetic = struct('lat',X(10),'long',X(11),'alt',X(12)); 31 | 32 | % ECEF position 33 | pos_ac_ecef =latlong_to_ecef(pos_ac_geodetic); 34 | 35 | % Velocity in ECEF Coordinates 36 | Velocity_ecef = ned_to_ecef(pos_ac_ecef,[X(1) X(2) X(3)]); 37 | 38 | end 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /MainGPS.asv: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%Main Run File%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 3 | % Global Positioning System Simulation Matlab Tool 4 | % Author: Saurav Agarwal 5 | % Email: saurav6@gmail.com 6 | % Date: January 1, 2011 7 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 8 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 9 | % Tips 10 | % All i/o units are specified in brackets 11 | % The terms user/aircraft have been used interchangeably 12 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 13 | % Features 14 | % 1. Plot Groundtrack of any GPS Satellite 15 | % 2. Track Single Satellite: Plot errors in propogation of signals, Skyplots, Elevation, No. of Satellites Visible 16 | % 3. Pure Dual Frequency GPS: Plot error in gps position/velocity for point to point flight at user defined altitude 17 | % 4. Carrier Phase DGPS autopilot landing (B747 aircraft) 18 | % 5. GPS + Flight Dynamics: Plot error in gps position/velocity for a flight with low dynamics 19 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 20 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 21 | % Load autopilot controller state space system 22 | clear all; 23 | clc; 24 | format long; 25 | load autopilotplant 26 | autopilotplant = errd2d; 27 | % load landing_trajectory 28 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 29 | % Constants % 30 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 31 | 32 | c = 2.99792458e8;% speed of light (m/s) 33 | rtd = 180/3.14159; % radians to degree 34 | dtr = 3.14159/180; 35 | initial_clk_bias = 1e-6; %(s) 36 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 37 | % Load Data % 38 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 39 | 40 | % Read GPS alamanac data from the m-file gps_alamanac_data 41 | gps_sat = gps_alamanac_data(); 42 | 43 | % Read locations of INRES data from the m-file inres_pos_data 44 | list_stations = inres_pos_data(); 45 | 46 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 47 | % Taking User Choices % 48 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 49 | fprintf(' List of Airports \n') 50 | fprintf(' 1. Delhi\n') 51 | fprintf(' 2. Jammu\n') 52 | fprintf(' 3. Ahmedabad\n') 53 | fprintf(' 4. Jodhpur\n') 54 | fprintf(' 5. Lucknow\n') 55 | fprintf(' 6. Bhopal \n') 56 | fprintf(' 7. Bagdogra\n') 57 | fprintf(' 8. Guwahati\n') 58 | fprintf(' 9. Aizwal \n') 59 | fprintf(' 10. Dibrugarh\n') 60 | fprintf(' 11. Raipur\n') 61 | fprintf(' 12. Kolkata\n') 62 | fprintf(' 13. Mumbai\n') 63 | fprintf(' 14. Hyderabad\n') 64 | fprintf(' 15. Vishakhapatnam\n') 65 | fprintf(' 16. Bangalore\n') 66 | fprintf(' 17. Chennai\n') 67 | fprintf(' 18. Agatti \n') 68 | fprintf(' 19. Trivandrum\n') 69 | fprintf(' 20. Port Blair\n') 70 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 71 | fprintf(' Please choose from the following list of modes of operation \n') 72 | fprintf(' 1. Plot Groundtrack of any GPS Satellite\n') 73 | fprintf(' 2. Track Single Satellite: Plot errors in propogation of signals, Skyplots, Elevation, No. of Satellites Visible\n') 74 | fprintf(' 3. Pure Dual Frequency GPS: Plot error in gps position for point to point flight at user defined altitude\n') 75 | fprintf(' 4. Carrier Phase DGPS Autopilot Landing (B747 Aircraft)\n') 76 | fprintf(' 5. GPS + Flight Dynamics: Plot error in gps position for a flight with low dynamics\n') 77 | 78 | mode = input('Please input your mode of operation (1,2,3,4,5):'); 79 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 80 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 81 | 82 | if mode == 1 83 | 84 | sv_id = input('Choose which satellite (1.2...31) to plot ground track:'); 85 | 86 | t_max = 24*3600; % Time of run for simulation 87 | h = 1000; % discretisation 88 | dt = t_max/h; % time-step 89 | 90 | for j =1:h 91 | 92 | gps_time = j*dt; 93 | 94 | [sv_x,sv_y,sv_z,Vsat_ecef] = calc_sat_pos_ecef(gps_sat,gps_time,sv_id) ;% The true position of satellite based on ephemeris data and the gps time 95 | 96 | true_sat_pos_ecef = struct('x',sv_x,'y',sv_y,'z',sv_z); 97 | 98 | sat_geodetic = ecef_to_latlong(true_sat_pos_ecef); % Geodetic Coordinates needed for plotting ground track 99 | 100 | lat_sv(j) = sat_geodetic.lat; 101 | long_sv(j) = sat_geodetic.long; 102 | end; 103 | 104 | 105 | % Plot ground track of any one satellite for 24 hrs % 106 | %Load the atlas to display groundtracks 107 | figure(1) 108 | worldmap('World') 109 | load coast 110 | plotm(lat,long) 111 | linem(lat_sv*rtd,long_sv*rtd); % Plot Ground Track 112 | 113 | end; 114 | 115 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 116 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 117 | 118 | if mode == 2 119 | 120 | station_id = input('Please choose your reference station (Enter the number of the airport given in list of airports):'); 121 | 122 | sv_id_track = input('Choose which satellite to track(1.2...31):'); % choose which satellite to track; 123 | 124 | true_user_pos_geodetic = list_stations(station_id); % coordinates in geodetic frame 125 | 126 | true_user_pos_ecef = latlong_to_ecef(true_user_pos_geodetic); % Coordinates of Reference station in geodetic system 127 | 128 | t_max = 12*3600; % Time of run for simulation 129 | 130 | visi_count = 0; 131 | 132 | for gps_time=1:1:t_max % gps_time is the true gps time 133 | 134 | current_time_hours = gps_time/3600 135 | 136 | time(gps_time) = gps_time; 137 | 138 | no_of_vis_sats = 0; 139 | 140 | % To compute the number of visible satellites from station 141 | for sv_id=1:31 % space vehicle number 142 | 143 | [sv_x,sv_y,sv_z] = calc_sat_pos_ecef(gps_sat,gps_time,sv_id); % The true position of satellite based on ephemeris data and the gps time 144 | 145 | true_sat_pos_ecef = struct('x',sv_x,'y',sv_y,'z',sv_z); 146 | 147 | [elev,azim,is_visible] = eval_el_az(true_user_pos_geodetic,true_user_pos_ecef,true_sat_pos_ecef); % gives elevation and azim in radians 148 | 149 | if is_visible == 1 150 | 151 | no_of_vis_sats = no_of_vis_sats + 1; % storing the number of visibile satellites 152 | end 153 | end; 154 | 155 | vis_sats(gps_time) = no_of_vis_sats; % storing the number of visibile satellites 156 | 157 | % Computing errors in measurement at station for selected satellite 158 | 159 | [sv_x,sv_y,sv_z] = calc_sat_pos_ecef(gps_sat,gps_time,sv_id_track); % The true position of satellite based on ephemeris data and the gps time 160 | 161 | track_sat_pos_ecef = struct('x',sv_x,'y',sv_y,'z',sv_z); 162 | 163 | [elev,azim,is_visible] = eval_el_az(true_user_pos_geodetic,true_user_pos_ecef,track_sat_pos_ecef); % gives elevation and azim in radians 164 | 165 | if is_visible == 1 166 | 167 | visi_count = visi_count+1; 168 | 169 | elevation_visi(visi_count) = elev; 170 | 171 | azimuth_visi(visi_count) = azim; 172 | 173 | time_visi(visi_count) = gps_time; 174 | 175 | slant_iono_delay_L1(visi_count) = eval_delay_iono(true_user_pos_geodetic,elev,azim,gps_time); % calculate L1 ionospheric delay in seconds 176 | 177 | slant_iono_delay_L2(visi_count) = 1.6469*slant_iono_delay_L1(visi_count); %calculate L2 ionospheric delay in seconds (from paper" a systems approach to ionospheric delay compenstaion") 178 | 179 | slant_tropo_delay(visi_count) = eval_delay_tropo(elev,true_user_pos_geodetic.alt); % calculate tropospheric delay in seconds 180 | 181 | rcvr_clk_error(visi_count) = 1*(10^-6)*randn; % white noise = clockppm*randomnumber 182 | 183 | 184 | end; 185 | 186 | 187 | end; 188 | %%%%%%%%%% Plots %%%%% 189 | figure(2) 190 | plot(time_visi/3600, slant_iono_delay_L1*c,'.') 191 | xlabel('Time (Hrs)') 192 | ylabel('L1 Iono Error (m)') 193 | figure(3) 194 | plot(time_visi/3600, slant_iono_delay_L2*c,'.') 195 | xlabel('Time (Hrs)') 196 | ylabel('L2 Iono Error (m)') 197 | figure(4) 198 | plot(time_visi/3600, rcvr_clk_error*c,'.') 199 | xlabel('Time (Hrs)') 200 | ylabel('Clock Error (m)') 201 | ylim([-1 +1]) 202 | figure(5) 203 | plot(time_visi/3600, slant_tropo_delay*c,'.') 204 | xlabel('Time (Hrs)') 205 | ylabel('Tropo Error (m)') 206 | figure(6) 207 | bar(time/3600, vis_sats) 208 | xlabel('Time (Hrs)') 209 | ylabel('No. of Satellites') 210 | figure(7) 211 | polar(azimuth_visi,(elevation_visi*rtd-90),'.') 212 | figure(8) 213 | plot(time_visi/3600,elevation_visi*rtd,'.') 214 | xlabel('Time (Hrs)') 215 | ylabel('Elevation Angle (Degrees)') 216 | 217 | end; 218 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 219 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 220 | 221 | if mode == 3 222 | 223 | start_airp_id = input('Please choose your starting point (Enter the number of the airport given in list of airports):'); 224 | 225 | end_airp_id = input('Please choose your ending point (Enter the number of the airport given in list of airports):'); 226 | 227 | ac_alt = input('Please input the altitude of flight (in m):'); % altitude of a/c in metres 228 | 229 | start_airp = list_stations(start_airp_id); % Geodetic Coordinates of start airport 230 | 231 | end_airp = list_stations(end_airp_id); % Geodetic Coordinates of end airport 232 | 233 | speed_ac = 800*1000/3600 ; %Speed of a commercial jet in m/s 234 | 235 | start_airp_ecef = latlong_to_ecef(start_airp); 236 | 237 | end_airp_ecef = latlong_to_ecef(end_airp); 238 | 239 | time_of_flight = round(compute_distance(start_airp_ecef,end_airp_ecef)/speed_ac); % time of flight in seconds 240 | 241 | roc_lat = (end_airp.lat - start_airp.lat)/time_of_flight ; % rate of change of latitude 242 | 243 | roc_long = (end_airp.long - start_airp.long)/time_of_flight ; % Rate of change of longitude 244 | 245 | initial_user_pos_estimate = struct('x',start_airp_ecef.x + 100*randn,'y',start_airp_ecef.y + 100*randn, 'z',start_airp_ecef.z + 100*randn); % A rough estimate of start position 246 | 247 | tcount = 0; 248 | 249 | start_time = 12*3600; 250 | 251 | R = ac_alt + 6378137; 252 | 253 | C_ECEF2NED = [-sin(start_airp.lat)*cos(start_airp.long) -sin(start_airp.lat)*sin(start_airp.long) cos(start_airp.lat);-sin(start_airp.long) cos(start_airp.long) 0; -cos(start_airp.lat)*cos(start_airp.long) -cos(start_airp.lat)*sin(start_airp.long) -sin(start_airp.lat)]; 254 | 255 | TrueVelocityECEF = (C_ECEF2NED'*R*[roc_lat;roc_long;0])'; 256 | 257 | estimate_user_vel_ecef = TrueVelocityECEF + randn(1,3); 258 | 259 | fprintf(' Please choose from the following list of modes of operation of GPS receiver \n') 260 | fprintf(' 1. All in view\n') 261 | fprintf(' 2. Tracking 4 satellites\n') 262 | GPSMODE = input('Please input the mode of operation of GPS receiver:'); % altitude of a/c in metres 263 | 264 | % Initialise 265 | optimum_sv_ids = zeros(time_of_flight,4); 266 | DOP = zeros(time_of_flight,4); 267 | error_in_pos = zeros(time_of_flight,1); 268 | error_in_x = zeros(time_of_flight,1); 269 | error_in_y = zeros(time_of_flight,1); 270 | error_in_z = zeros(time_of_flight,1); 271 | number_of_visible_sats = zeros(time_of_flight,1); 272 | ErrorSpeed = zeros(time_of_flight,3); 273 | time = zeros(time_of_flight,1); 274 | ac_lat = zeros(time_of_flight,1); 275 | ac_long = zeros(time_of_flight,1); 276 | 277 | for gps_time= start_time:1:start_time+time_of_flight % gps_time is the true gps time 278 | 279 | tcount = tcount +1; 280 | 281 | ac_lat(tcount) = start_airp.lat + roc_lat*tcount ; % latitude of aircraft in radians 282 | 283 | ac_long(tcount) = start_airp.long + roc_long*tcount; % longitude of aircraft in radians 284 | 285 | true_user_pos_geodetic = struct('lat',ac_lat(tcount),'long',ac_long(tcount),'alt',ac_alt); % user coordinates in geodetic frame 286 | 287 | true_user_pos_ecef = latlong_to_ecef(true_user_pos_geodetic); %user coordinates in ECEF frame 288 | 289 | current_time_hours = gps_time/3600 290 | 291 | visible_sats_id = []; % To store the ids of visible satellites 292 | 293 | time(tcount) = gps_time; 294 | 295 | no_of_vis_sats = 0; 296 | 297 | C_ECEF2NED = [-sin(ac_lat(tcount))*cos(ac_long(tcount)) -sin(ac_lat(tcount))*sin(ac_long(tcount)) cos(ac_lat(tcount));-sin(ac_long(tcount)) cos(ac_long(tcount)) 0; -cos(ac_lat(tcount))*cos(ac_long(tcount)) -cos(ac_lat(tcount))*sin(ac_long(tcount)) -sin(ac_lat(tcount))]; 298 | 299 | TrueVelocityECEF = (C_ECEF2NED'*R*[roc_lat;roc_long;0])'; 300 | 301 | 302 | for sv_id=1:31 % space vehicle number 303 | 304 | [sv_x,sv_y,sv_z] = calc_sat_pos_ecef(gps_sat,gps_time,sv_id); % The true position of satellite based on ephemeris data and the gps time 305 | 306 | true_sat_pos_ecef = struct('x',sv_x,'y',sv_y,'z',sv_z); 307 | 308 | [elev,azim,is_visible] = eval_el_az(true_user_pos_geodetic,true_user_pos_ecef,true_sat_pos_ecef); % gives elevation and azim in radians 309 | 310 | if is_visible == 1 311 | 312 | SV_Elevation_Angles(tcount,sv_id) = elev*rtd; 313 | visible_sats_id = [visible_sats_id sv_id]; % create a list of the ids of visible satellites 314 | no_of_vis_sats = no_of_vis_sats + 1; 315 | end; 316 | if is_visible == 0 317 | 318 | SV_Elevation_Angles(tcount,sv_id) = 0; 319 | 320 | end; 321 | 322 | end; 323 | number_of_visible_sats(tcount) = no_of_vis_sats; 324 | [user_pos_gps,optimum_sv_ids(tcount,:), DOP(tcount,:),estimate_user_vel_ecef] = Dual_Freq_GPS(GPSMODE,gps_sat,gps_time,visible_sats_id,true_user_pos_ecef, initial_user_pos_estimate,inres_pos_data,estimate_user_vel_ecef,TrueVelocityECEF); 325 | initial_user_pos_estimate = user_pos_gps; % new estimate becomes the previous gps position 326 | ErrorSpeed(tcount,:) = estimate_user_vel_ecef - TrueVelocityECEF; 327 | error_in_pos(tcount) = compute_distance(user_pos_gps,true_user_pos_ecef); %user_pos_gps.x - true_user_pos_ecef.x ; % what the receiver thinks is true pseduo range 328 | error_in_x(tcount) = user_pos_gps.x - true_user_pos_ecef.x ; 329 | error_in_y(tcount) = user_pos_gps.y - true_user_pos_ecef.y ; 330 | error_in_z(tcount) = user_pos_gps.z - true_user_pos_ecef.z ; 331 | end; 332 | RMS_Error = sqrt(norm(error_in_pos)^2/length(error_in_pos)); 333 | %%%%%%%%%%%%%%%%% Plots %%%%%%%%%%%%%%%%%%%%%%%%%% 334 | figure(8) 335 | plot(time/3600,error_in_pos,'.') 336 | xlabel('Time(Hrs)') 337 | ylabel('Error in Pos (m)') 338 | figure(9) 339 | plot(time/3600,error_in_x,'.') 340 | xlabel('Time(Hrs)') 341 | ylabel('Error in X (m)') 342 | figure(10) 343 | plot(time/3600,error_in_y,'.') 344 | xlabel('Time(Hrs)') 345 | ylabel('Error in Y (m)') 346 | figure(11) 347 | plot(time/3600,error_in_z,'.') 348 | xlabel('Time(Hrs)') 349 | ylabel('Error in Z (m)') 350 | figure(12) 351 | plot(time/3600,number_of_visible_sats) 352 | xlabel('Time(Hrs)') 353 | ylabel('Number of Visible Satellites') 354 | ylim([4 15]) 355 | figure(13) 356 | plot(time/3600, DOP) 357 | xlabel('Time(Hrs)') 358 | ylabel('DOP') 359 | legend('GDOP','PDOP','HDOP','VDOP') 360 | figure(14) 361 | plot(time/3600,SV_Elevation_Angles) 362 | xlabel('Time(Hrs)') 363 | ylabel('Elevation (degrees)') 364 | legend('SV1','SV2','SV3','SV4','SV5','SV6','SV7','SV8','SV9','SV10','SV11','SV12','SV13','SV14','SV15','SV16','SV17','SV18','SV19','SV20','SV21','SV22','SV23','SV24','SV25','SV26','SV27','SV28','SV29','SV30','SV31'); 365 | ylim([10 90]) 366 | figure(15) 367 | plot(time/3600, ErrorSpeed) 368 | xlabel('Time(Hrs)') 369 | ylabel('Error in Speed') 370 | legend('V_x','V_y','V_z') 371 | figure(16) 372 | worldmap('World') 373 | load coast 374 | plotm(lat, long) 375 | linem(ac_lat*rtd,ac_long*rtd); % Plot Ground Track 376 | 377 | end; 378 | 379 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 380 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 381 | % Landing With Integrity Beacons 382 | % Aircraft does a bubble pass at constant altitute over integrity beacons 383 | % and then switches to carrier phase D-GPS after requisite epochs and 384 | % starts descending on preset glide slope 385 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 386 | if mode == 4 387 | 388 | 389 | ref_station_id = input('Please choose your destination airport (Enter the number of the airport given in list of airports):'); % end airport has reference receiver 390 | 391 | ref_station = list_stations(ref_station_id); % Geodetic Coordinates of end airport 392 | 393 | ref_station_ecef = latlong_to_ecef(ref_station); 394 | 395 | ibeacon1_geo = struct('lat',ref_station.lat - 0.0025,'long',ref_station.long +.0004,'alt',ref_station.alt); % ~16 km from airport geodetic position of integrity beacon in goedetic coordinates 396 | 397 | ibeacon1_ecef = latlong_to_ecef(ibeacon1_geo); 398 | 399 | ibeacon2_geo = struct('lat',ref_station.lat - 0.0025,'long',ref_station.long -.0004,'alt',ref_station.alt); % geodetic position of integrity beacon in goedetic coordinates 400 | 401 | ibeacon2_ecef = latlong_to_ecef(ibeacon2_geo); 402 | 403 | compute_distance(ibeacon2_ecef,ref_station_ecef) 404 | 405 | ac_start_altitude = ref_station.alt+ 800; % metres 406 | 407 | speed_ac = 221*0.3048 ; % Speed of Boeing 747 in m/s during approach 408 | 409 | start_ac_pos_geodetic = struct('lat',ref_station.lat - 0.0030,'long',ref_station.long,'alt',ac_start_altitude); % ~26 km from airport, starting position of A/C in goedetic coordinates 410 | 411 | start_ac_ecef = latlong_to_ecef(start_ac_pos_geodetic); 412 | 413 | time_of_flight = 550;%compute_distance(start_ac_ecef,ref_station_ecef)/speed_ac; % time of flight in seconds 414 | 415 | roc_lat = (ref_station.lat - start_ac_pos_geodetic.lat)/time_of_flight ; % rate of change of latitude 416 | 417 | roc_long = (ref_station.long - start_ac_pos_geodetic.long)/time_of_flight ; % Rate of change of longitude 418 | 419 | initial_user_pos_estimate = struct('x',start_ac_ecef.x + 50,'y',start_ac_ecef.y + 50, 'z',start_ac_ecef.z + 50); % A rough estimate of start position 420 | 421 | data_collected = 0; % the nature of phase measurement data from reference station and integrity beacons 422 | 423 | no_of_measurement_epochs = 100; 424 | 425 | countme = 0; 426 | 427 | time_interval = 1; % seconds intervals at which measurements are taken 428 | 429 | tcount = 0; 430 | 431 | rod = (ac_start_altitude-ref_station.alt)/(time_of_flight-no_of_measurement_epochs*time_interval); % rate of decent 432 | 433 | C_ECEF2NED = [-sin(ref_station.lat)*cos(ref_station.long) -sin(ref_station.lat)*sin(ref_station.long) cos(ref_station.lat);-sin(ref_station.long) cos(ref_station.long) 0; -cos(ref_station.lat)*cos(ref_station.long) -cos(ref_station.lat)*sin(ref_station.long) -sin(ref_station.lat)]; 434 | 435 | TrueVelocityECEF = [0 0 0];%(C_ECEF2NED'*R*[roc_lat;roc_long;0])'; 436 | 437 | estimate_user_vel_ecef = [0 0 0];%TrueVelocityECEF + randn(1,3); 438 | 439 | initial_clk_bias_u = 1e-6; 440 | initial_clk_bias_r = 1e-6; 441 | autpilot_timer = 0; 442 | jcounter = 0; 443 | dt = 0.01; 444 | X(1,:) = [0 0 0 0 0 0 0 221 0 0 0 0 0 0 0]; 445 | % glideslope_alt = linspace(0,0,time_of_flight/0.01); 446 | 447 | for gps_time = 0:0.01:round(time_of_flight) % gps_time is the true gps time 448 | 449 | current_time_seconds = gps_time 450 | 451 | tcount = tcount + 1; 452 | 453 | ac_lat(tcount) = start_ac_pos_geodetic.lat + roc_lat*gps_time ;% latitude of aircraft in radians 454 | 455 | ac_long(tcount) = start_ac_pos_geodetic.long + roc_long*gps_time; % longitude of aircraft in radians 456 | 457 | ac_true_lat(tcount) = start_ac_pos_geodetic.lat + (221*gps_time+X(tcount,7))*0.3048/6378137 ; 458 | 459 | ac_true_long(tcount) = start_ac_pos_geodetic.long ; 460 | 461 | ac_true_height(tcount) = start_ac_pos_geodetic.alt + X(tcount,6)*0.3048; 462 | 463 | true_user_pos_geodetic = struct('lat',ac_true_lat(tcount),'long',ac_true_long(tcount),'alt',ac_true_height(tcount)); % user coordinates in geodetic frame 464 | 465 | true_user_pos_ecef = latlong_to_ecef(true_user_pos_geodetic); % user coordinates in ECEF frame 466 | 467 | 468 | if sqrt(-(ac_start_altitude)^2*(gps_time-time_of_flight)/time_of_flight) - ref_station.alt > 50 469 | glideslope_alt(tcount) = sqrt(-(ac_start_altitude)^2*(gps_time-time_of_flight)/time_of_flight); % parabolic descent path 470 | 471 | end; 472 | 473 | if gps_time > 200 474 | 475 | if sqrt(-(ac_start_altitude)^2*(gps_time-time_of_flight)/time_of_flight) - ref_station.alt <= 50 476 | 477 | glideslope_alt(tcount) = glideslope_alt(tcount-1)*exp(-2e-6*(glideslope_alt(tcount-1)-ref_station.alt+15)); % flare phase 478 | end; 479 | 480 | end; 481 | 482 | 483 | visible_sats_id = []; % To store the ids of visible satellites 484 | 485 | time(tcount) = gps_time; 486 | 487 | % Distance_from_ibeacon1 = compute_distance(true_user_pos_ecef,ibeacon1_ecef)/1000 488 | % 489 | % Distance_from_airport(tcount) = (ref_station.lat-ac_true_lat(tcount))*6378.137; 490 | 491 | % Altitude_above_ref_station(tcount) = ac_alt(tcount)-ref_station.alt; 492 | 493 | for sv_id=1:31 % space vehicle number 494 | 495 | [sv_x,sv_y,sv_z] = calc_sat_pos_ecef(gps_sat,gps_time,sv_id); % The true position of satellite based on ephemeris data and the gps time 496 | 497 | true_sat_pos_ecef = struct('x',sv_x,'y',sv_y,'z',sv_z); 498 | 499 | [elev,azim,is_visible] = eval_el_az(true_user_pos_geodetic,true_user_pos_ecef,true_sat_pos_ecef); % gives elevation and azim in radians from user to gpssat 500 | 501 | [elev_ref,azim_ref,is_visible_ref] = eval_el_az(ref_station,ref_station_ecef,true_sat_pos_ecef); % gives elevation and azim in radians from reference station to gpssat 502 | 503 | [elev_ib1,azim_ib1,is_visible_ib1] = eval_el_az(ibeacon1_geo,ibeacon1_ecef,true_sat_pos_ecef); 504 | 505 | [elev_ib2,azim_ib2,is_visible_ib2] = eval_el_az(ibeacon2_geo,ibeacon2_ecef,true_sat_pos_ecef); 506 | 507 | if is_visible == 1 && is_visible_ref == 1 && is_visible_ib1 ==1 && is_visible_ib2 == 1 508 | 509 | visible_sats_id = [visible_sats_id sv_id]; % create a list of the ids of satellites visible from both stations 510 | 511 | end; 512 | 513 | end; 514 | 515 | no_of_visible_sats = length(visible_sats_id); 516 | 517 | if data_collected == 0 518 | if rem(gps_time,time_interval)==0 519 | [user_pos_gps,optimum_sv_ids,DOP,Velocity_Ecef,initial_clk_bias] = Dual_Freq_GPS(initial_clk_bias,2,gps_sat,gps_time,visible_sats_id,true_user_pos_ecef, initial_user_pos_estimate,inres_pos_data,estimate_user_vel_ecef,TrueVelocityECEF); 520 | initial_user_pos_estimate = user_pos_gps; 521 | user_pos_calculated = user_pos_gps; 522 | user_pos_calculated_latlong = ecef_to_latlong(user_pos_calculated); 523 | jcounter = jcounter+1; 524 | d(jcounter) = user_pos_calculated_latlong.alt - glideslope_alt(tcount); 525 | 526 | countme = countme+1; 527 | time_of_epochs(countme) = gps_time; 528 | true_user_pos_geodetic_storage(countme) = true_user_pos_geodetic; 529 | true_user_pos_ecef_storage(countme) = true_user_pos_ecef; 530 | initial_user_pos_estimate_storage(countme) = initial_user_pos_estimate; 531 | end; 532 | end; 533 | 534 | if countme == no_of_measurement_epochs && data_collected ==0 535 | data_collected = 1; 536 | [IntegerAmbiguities,AA,initial_clk_bias_u,initial_clk_bias_r] = eval_integer_ambiguity(no_of_measurement_epochs,time_of_epochs,countme,gps_sat,visible_sats_id,ref_station_ecef,ref_station,initial_user_pos_estimate_storage,true_user_pos_geodetic_storage,true_user_pos_ecef_storage,ibeacon1_geo,ibeacon1_ecef,ibeacon2_geo,ibeacon2_ecef,initial_clk_bias_u,initial_clk_bias_r); 537 | end; 538 | 539 | if data_collected == 1 540 | 541 | if rem(gps_time,1) == 0 542 | 543 | [user_pos_cdgps2] = estimate_user_position_cdgps2(IntegerAmbiguities,gps_sat,gps_time,visible_sats_id,ref_station_ecef,ref_station,true_user_pos_geodetic,true_user_pos_ecef, initial_user_pos_estimate, ibeacon1_geo,ibeacon1_ecef,ibeacon2_geo,ibeacon2_ecef,initial_clk_bias_u,initial_clk_bias_r); % This function estimates the position of the user based on Carrier Phase Differential GPS 544 | 545 | initial_user_pos_estimate = user_pos_cdgps2; % new estimate becomes the previous gps position 546 | 547 | user_pos_calculated = user_pos_cdgps2; 548 | 549 | user_pos_calculated_latlong = ecef_to_latlong(user_pos_calculated); 550 | jcounter = jcounter + 1 551 | d(jcounter) = user_pos_calculated_latlong.alt - glideslope_alt(tcount); 552 | end; 553 | 554 | 555 | end; 556 | 557 | 558 | Z = X(tcount,:)+ randn(1,15); % measurement matrix 559 | Z(5) = d(jcounter)/0.3048; % d measurement comes from gps 560 | 561 | 562 | X(tcount+1,:) = RK4_autopilot(dt,X(tcount,:),Z,autopilotplant); 563 | user_pos_calculated_geodetic = ecef_to_latlong(user_pos_calculated); 564 | error_in_east(tcount) = (true_user_pos_geodetic.long - user_pos_calculated_geodetic.long)*6378137; 565 | error_in_down(tcount) = true_user_pos_geodetic.alt - user_pos_calculated_geodetic.alt; 566 | error_in_pos(tcount) = compute_distance(user_pos_calculated,true_user_pos_ecef); %user_pos_gps.x - true_user_pos_ecef.x ; % what the receiver thinks is true pseduo range 567 | error_in_x(tcount) = true_user_pos_ecef.x - user_pos_calculated.x ; 568 | error_in_y(tcount) = true_user_pos_ecef.y - user_pos_calculated.y ; 569 | error_in_z(tcount) = true_user_pos_ecef.z - user_pos_calculated.z ; 570 | 571 | end; 572 | % RMS_Error = sqrt(norm(error_in_pos)^2/length(error_in_pos)) 573 | % Ambiguities = round(IntegerAmbiguities') 574 | % AA = AA/0.19; 575 | % MeanError_Vertical_DGPS = mean(error_in_down(no_of_measurement_epochs*time_interval:end)) 576 | % StdDev_Error_Vertical_DGPS = std(error_in_down(no_of_measurement_epochs*time_interval:end)) 577 | % MeanError_Horizontal_DGPS = mean(error_in_east(no_of_measurement_epochs*time_interval:end)) 578 | % StdDev_Error_Horizontal_DGPS = std(error_in_east(no_of_measurement_epochs*time_interval:end)) 579 | Comparison_alt = [glideslope_alt' ac_true_height']; 580 | %%%%%%%%%%%%%%%%% Plots %%%%%%%%%%%%%%%%%%%%%%%%%% 581 | plot(time,glideslope_alt-200) 582 | % figure(1) 583 | % plot(X(:,5)*0.3048) 584 | figure(2) 585 | plot(time,Comparison_alt-ref_station.alt); 586 | legend('Desired Glideslope','Aircraft Trajectory') 587 | xlabel('time') 588 | ylabel('altitude (m)') 589 | figure(3) 590 | Error_traj = ac_true_height - glideslope_alt; 591 | plot(time,Error_traj) 592 | ylabel('Vertical Deviation from Glideslope') 593 | xlabel('time') 594 | % figure(13) 595 | % plot(time,error_in_pos,'.') 596 | % xlabel('Time(s)') 597 | % ylabel('Error in Pos (m)') 598 | % figure(14) 599 | % plot(time,error_in_x,'.') 600 | % xlabel('Time(s)') 601 | % ylabel('Error in X (m)') 602 | % figure(15) 603 | % plot(time,error_in_y,'.') 604 | % xlabel('Time(s)') 605 | % ylabel('Error in Y (m)') 606 | % figure(16) 607 | % plot(time,error_in_z,'.') 608 | % xlabel('Time(s)') 609 | % ylabel('Error in Z (m)') 610 | end; 611 | 612 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 613 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 614 | if mode == 5 615 | 616 | 617 | % Selecting GPS mode 618 | fprintf(' Please choose from the following list of modes of operation of GPS receiver \n') 619 | fprintf(' 1. All in view\n') 620 | fprintf(' 2. Tracking 4 satellites\n') 621 | GPSMODE = input('Please input the mode of operation of GPS receiver:'); % altitude of a/c in metres 622 | 623 | start_airp_id = 1;%input('Please choose your starting point (Enter the number of the airport given in list of airports):'); 624 | 625 | start_airp = list_stations(start_airp_id); % Geodetic Coordinates of start airport 626 | 627 | ac_alt = 1000;%input('Please input the altitude of flight (in m):'); % altitude of a/c in metres 628 | 629 | true_user_pos_geodetic = struct('lat',start_airp.lat,'long',start_airp.long,'alt',ac_alt); % user coordinates in geodetic frame 630 | 631 | true_user_pos_ecef = latlong_to_ecef(true_user_pos_geodetic); 632 | 633 | % Defining the initial state of the aircraft 634 | % Where state X = [Vnorth Veast Vdown Phi Theta Psi P Q R lat long alt] 635 | % euler angles Phi Theta Psi are w.r.t local navigation frame 636 | X = [40 0 0 0 0 0 0 0 0 true_user_pos_geodetic.lat true_user_pos_geodetic.long true_user_pos_geodetic.alt]; 637 | 638 | % Define Control Settings 639 | dth = 240; %('Throttle (95-500):'); 640 | dr = 0; % Rudder deflection in degrees 641 | da = 0; % Aileron deflection in degrees 642 | de = 2; % Elevator deflection in degrees 643 | 644 | time_of_flight = 1000;%input('Please input the time of flight (in seconds):'); % time of flight in seconds 645 | 646 | dt = 0.01; 647 | 648 | initial_user_pos_estimate = struct('x',true_user_pos_ecef.x + 500*randn,'y',true_user_pos_ecef.y + 500*randn, 'z',true_user_pos_ecef.z + 500*randn); % A rough estimate of start position 649 | 650 | gpscounter = 0; 651 | 652 | tcount= 0; 653 | 654 | %%% Initialise the storage arrays, results in faster iterations 655 | TrueAccelerationBody = zeros(4,3,time_of_flight/dt); 656 | TrueAngularVelocity = zeros(4,3,time_of_flight/dt); 657 | TruePos_Geodetic = zeros(time_of_flight/dt,3); 658 | TrueVelocityNED = zeros(time_of_flight/dt,3); 659 | TrueAttitude = zeros(time_of_flight/dt,3); 660 | TrueVelocityECEF = zeros(time_of_flight/dt,3); 661 | VelocityGPS_ECEF = zeros(time_of_flight,3); 662 | optimum_sv_ids = zeros(time_of_flight,4); 663 | DOP = zeros(time_of_flight,4); 664 | error_in_pos = zeros(time_of_flight,1); 665 | error_in_x = zeros(time_of_flight,1); 666 | error_in_y = zeros(time_of_flight,1); 667 | error_in_z = zeros(time_of_flight,1); 668 | ErrorVelocity_NED = zeros(time_of_flight,3); 669 | time = zeros(time_of_flight,1); 670 | VelocityGPS_NED = zeros(time_of_flight,3); 671 | TrueVelocityNED(1,:) = X(1:3); 672 | TrueAttitude(1,:) = X(4:6); 673 | TruePos_Geodetic(1,:) = X(10:12); 674 | ErrorVelocity_ECEF = zeros(time_of_flight,3); 675 | 676 | C_ECEF2NED = [-sin(X(10))*cos(X(11)) -sin(X(10))*sin(X(11)) cos(X(10));-sin(X(11)) cos(X(11)) 0; -cos(X(10))*cos(X(11)) -cos(X(10))*sin(X(11)) -sin(X(10))]; 677 | 678 | TrueVelocityECEF(1,:) = (C_ECEF2NED'*TrueVelocityNED(1,:)')'; 679 | 680 | estimate_user_vel_ecef = TrueVelocityECEF(tcount+1,:) + randn(1,3); 681 | 682 | 683 | for gps_time=0:dt:time_of_flight % gps_time is the true gps time 684 | 685 | current_time_hours = gps_time 686 | 687 | tcount = tcount+1; 688 | 689 | [true_user_pos_ecef, true_user_pos_geodetic,TrueAccelerationBody(:,:,tcount),TrueAngularVelocity(:,:,tcount),X] = FlightMech_Model(dt,X,dth,de,da,dr); 690 | 691 | TruePos_Geodetic(tcount+1,:) = X(10:12); 692 | 693 | Altitude = X(12) 694 | 695 | TrueVelocityNED(tcount+1,:) = X(1:3); 696 | 697 | TrueAttitude(tcount+1,:) = X(4:6); 698 | 699 | C_ECEF2NED = [-sin(X(10))*cos(X(11)) -sin(X(10))*sin(X(11)) cos(X(10));-sin(X(11)) cos(X(11)) 0; -cos(X(10))*cos(X(11)) -cos(X(10))*sin(X(11)) -sin(X(10))]; 700 | 701 | TrueVelocityECEF(tcount+1,:) = (C_ECEF2NED'*TrueVelocityNED(tcount+1,:)')'; 702 | 703 | if X(12) < 10 704 | fprintf('CRASH!!! \n') 705 | break; 706 | end; 707 | 708 | visible_sats_id = []; % To store the ids of visible satellites 709 | 710 | no_of_vis_sats = 0; 711 | 712 | % GPS updates come once a second 713 | if rem(gps_time,1)== 0 714 | gpscounter = gpscounter +1; 715 | time(gpscounter) = gps_time; 716 | ac_lat(gpscounter) = true_user_pos_geodetic.lat; 717 | ac_long(gpscounter) = true_user_pos_geodetic.long; 718 | for sv_id=1:31 % space vehicle number 719 | 720 | [sv_x,sv_y,sv_z] = calc_sat_pos_ecef(gps_sat,gps_time,sv_id); % The true position of satellite based on ephemeris data and the gps time 721 | 722 | true_sat_pos_ecef = struct('x',sv_x,'y',sv_y,'z',sv_z); 723 | 724 | [elev,azim,is_visible] = eval_el_az(true_user_pos_geodetic,true_user_pos_ecef,true_sat_pos_ecef); % gives elevation and azim in radians 725 | 726 | if is_visible == 1 727 | SV_Elevation_Angles(gpscounter,sv_id) = elev*rtd; 728 | visible_sats_id = [visible_sats_id sv_id]; % create a list of the ids of visible satellites 729 | no_of_vis_sats = no_of_vis_sats + 1; 730 | end; 731 | 732 | if is_visible == 0 733 | 734 | SV_Elevation_Angles(gpscounter,sv_id) = 0; 735 | 736 | end; 737 | 738 | end; 739 | % Use GPS to estimate Position and velocity 740 | [user_pos_gps,optimum_sv_ids(gpscounter,:),DOP(gpscounter,:),VelocityGPS_ECEF(gpscounter,:),initial_clk_bias] = Dual_Freq_GPS(initial_clk_bias,GPSMODE,gps_sat,gps_time,visible_sats_id,true_user_pos_ecef, initial_user_pos_estimate,inres_pos_data,estimate_user_vel_ecef,TrueVelocityECEF(tcount+1,:)); 741 | 742 | estimate_user_vel_ecef = VelocityGPS_ECEF(gpscounter,:); 743 | 744 | VelocityGPS_NED(gpscounter,:) = (C_ECEF2NED*VelocityGPS_ECEF(gpscounter,:)')'; 745 | 746 | ErrorVelocity_ECEF(gpscounter,:) = VelocityGPS_ECEF(gpscounter,:)-TrueVelocityECEF(tcount+1,:); 747 | 748 | ErrorVelocity_NED(gpscounter,:) = (C_ECEF2NED*(VelocityGPS_ECEF(gpscounter,:)-TrueVelocityECEF(tcount+1,:))')'; % VelocityGPS_NED(gpscounter,:) - TrueVelocityNED(tcount+1,:); % Error in estimated velocity in north,east,down 749 | 750 | initial_user_pos_estimate = user_pos_gps; % new gps position becomes the estimate for next iteration 751 | 752 | number_of_visible_sats(gpscounter) = no_of_vis_sats; 753 | 754 | error_in_pos(gpscounter) = compute_distance(user_pos_gps,true_user_pos_ecef); 755 | 756 | error_in_x(gpscounter) = user_pos_gps.x - true_user_pos_ecef.x ; 757 | 758 | error_in_y(gpscounter) = user_pos_gps.y - true_user_pos_ecef.y; 759 | 760 | error_in_z(gpscounter) = user_pos_gps.z - true_user_pos_ecef.z ; 761 | 762 | end; 763 | 764 | end; 765 | RMS_Error = sqrt(norm(error_in_pos)^2/length(error_in_pos)); 766 | %%%%%%%%%%%%%%%%% Plots %%%%%%%%%%%%%%%%%%%%%%%%%% 767 | figure(8) 768 | plot(time,error_in_pos,'.') 769 | xlabel('Time(Hrs)') 770 | ylabel('Error in Pos (m)') 771 | figure(9) 772 | plot(time,error_in_x,'.') 773 | xlabel('Time(Hrs)') 774 | ylabel('Error in X (m)') 775 | figure(10) 776 | plot(time,error_in_y,'.') 777 | xlabel('Time(Hrs)') 778 | ylabel('Error in Y (m)') 779 | figure(11) 780 | plot(time,error_in_z,'.') 781 | xlabel('Time(Hrs)') 782 | ylabel('Error in Z (m)') 783 | figure(12) 784 | plot(time,number_of_visible_sats) 785 | xlabel('Time(Hrs)') 786 | ylabel('Number of Visible Satellites') 787 | ylim([4 15]) 788 | figure(13) 789 | plot(time, DOP) 790 | xlabel('Time(Hrs)') 791 | ylabel('DOP') 792 | legend('GDOP','PDOP','HDOP','VDOP') 793 | figure(14) 794 | plot(time,Altitude) 795 | xlabel('Time(Hrs)') 796 | ylabel('Altitude (m)') 797 | figure(15) 798 | plot(time,SV_Elevation_Angles) 799 | xlabel('Time(Hrs)') 800 | ylabel('Elevation (degrees)') 801 | legend('SV1','SV2','SV3','SV4','SV5','SV6','SV7','SV8','SV9','SV10','SV11','SV12','SV13','SV14','SV15','SV16','SV17','SV18','SV19','SV20','SV21','SV22','SV23','SV24','SV25','SV26','SV27','SV28','SV29','SV30','SV31'); 802 | figure(16) 803 | plot(time/3600, ErrorVelocity_ECEF) 804 | xlabel('Time(Hrs)') 805 | ylabel('Error in Velocity (ECEF)') 806 | legend('V_x','V_y','V_z') 807 | figure(17) 808 | plot(ErrorVelocity_NED(1:end,2)*1e2,ErrorVelocity_NED(1:end,1)*1e2,'+') 809 | xlabel('East Error (cm/s)') 810 | ylabel('North Error (cm/s)') 811 | % % Plot ground track of aeroplane %% 812 | % figure(17) 813 | % worldmap('World') 814 | % load coast 815 | % plotm(lat, long) 816 | % linem(ac_lat*rtd,ac_long*rtd); % Plot Ground Track 817 | 818 | end; -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | --------------------------------------- 2 | License: 3 | ---------------------------------------- 4 | Software License Agreement (BSD License) 5 | 6 | Copyright (c) 2013, Saurav Agarwal 7 | All rights reserved. 8 | 9 | Redistribution and use in source and binary forms, with or without 10 | modification, are permitted provided that the following conditions 11 | are met: 12 | 13 | * Redistributions of source code must retain the above copyright 14 | notice, this list of conditions and the following disclaimer. 15 | * Redistributions in binary form must reproduce the above 16 | copyright notice, this list of conditions and the following 17 | disclaimer in the documentation and/or other materials provided 18 | with the distribution. 19 | * Neither the name of the Indian Institute of Technology, Bombay nor the names of its 20 | contributors may be used to endorse or promote products derived 21 | from this software without specific prior written permission. 22 | 23 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | POSSIBILITY OF SUCH DAMAGE. 35 | 36 | 37 | GPSMATLAB 38 | ---------------------------------------- 39 | MATLAB Toolbox for simulating Single/Dual Frequency GPS and Carrier Phase Differential GPS. 40 | 41 | Contact 42 | ---------------------------------------- 43 | Visit: http://sauravag.com 44 | 45 | References: 46 | ---------------------------------------- 47 | 1. S Agarwal and H B Hablani., “Automatic Aircraft Landing over Parabolic Trajectory using Precise GPS Measurements,” IJCA Proceedings on International Conference and workshop on Emerging Trends in Technology (ICWET) (7):38-45, 2011 48 | 49 | 2. Agarwal, S. & Hablani, H.B., “Precise Positioning Using GPS for CAT-III Aircraft Operations Using Smoothed Pseudorange Measurements,” International Conference and Workshop on Emerging Trends in Technology, February 25-26, 2011, Mumbai, India 50 | -------------------------------------------------------------------------------- /RK4_autopilot.asv: -------------------------------------------------------------------------------- 1 | % RK4 INTEGRATION FUNCTION 2 | function[Xnew] = RK4_autopilot(dt,X,autopilotplant) 3 | 4 | XA = linspace(0,0,length(X)); 5 | XB = XA; 6 | DX = acplant(X,autopilotplant); 7 | XA = DX * dt; 8 | XB = X + 0.5 * XA; 9 | DX = acplant(X,autopilotplant); 10 | Q = DX * dt; 11 | XB = X + 0.5 * Q; 12 | XA = XA + 2.0 * Q; 13 | DX = acplant(X,autopilotplant); 14 | Q = DX * dt; 15 | XB = X + Q; 16 | XA = XA + 2.0 * Q; 17 | DX = acplant(X,autopilotplant); 18 | Xnew = X + (XA + DX * dt)/6.0; 19 | end 20 | 21 | function [DX] = acplant(X,Z,autopilotplant) 22 | H = eye(15,15); % measurement matrix 23 | K = [1 1 1 1 1 1 1 1 1 1 1 1 1 1 1]; 24 | Bw = [0.0210 -0.122;0.2090 0.5300;-0.0170 0.1640;zeros(12,2)]; 25 | [a,b,c,d] = ssdata(autopilotplant); 26 | size(a*X') 27 | DX = (a*X' + b*-[0 0 0 0 1 0 0 0 0 0 0 0 0 0 0]*Z' + Bw*[10+randn;5+randn])'; 28 | 29 | end -------------------------------------------------------------------------------- /RK4_autopilot.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % RK4 INTEGRATION FUNCTION for autopilot 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | 9 | 10 | function[Xnew] = RK4_autopilot(dt,X,Z,autopilotplant) 11 | 12 | XA = linspace(0,0,length(X)); 13 | XB = XA; 14 | DX = acplant(X,Z,autopilotplant); 15 | XA = DX * dt; 16 | XB = X + 0.5 * XA; 17 | DX = acplant(X,Z,autopilotplant); 18 | Q = DX * dt; 19 | XB = X + 0.5 * Q; 20 | XA = XA + 2.0 * Q; 21 | DX = acplant(X,Z,autopilotplant); 22 | Q = DX * dt; 23 | XB = X + Q; 24 | XA = XA + 2.0 * Q; 25 | DX = acplant(X,Z,autopilotplant); 26 | Xnew = X + (XA + DX * dt)/6.0; 27 | end 28 | 29 | function [DX] = acplant(X,Z,autopilotplant); 30 | % H = eye(15,15); % measurement matrix 31 | % K = [1 1 1 1 1 1 1 1 1 1 1 1 1 1 1]; 32 | Bw = [0.0210 -0.122;0.2090 0.5300;-0.0170 0.1640;zeros(12,2)]; 33 | [a,b,c,d] = ssdata(autopilotplant); 34 | DX = (a*X' - b*[0 0 0 0 1 0 0 0 0 0 0 0 0 0 0]*Z' + Bw*[15+randn;5+randn])'; 35 | 36 | end -------------------------------------------------------------------------------- /atmosphere.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Atmosphere Function % 3 | % Author: Saurav Agarwal % 4 | % Date: January 1, 2011 % 5 | % Dept. of Aerospace Engg., IIT Bombay, Mumbai, India % 6 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 7 | 8 | % FUNCTION TO CALCULATE 'qbar' 9 | function [qbar,rho] = atmosphere(alt,Vt) 10 | 11 | %** CONSTANT PARAMETERS ** 12 | g = 9.86; % [m/s^2] 13 | temp_lapserate = 0.005; % [K/m] 14 | temp_sealevel = 300; % Temperature at sealevel[K] 15 | pressure_sealevel = 101325; % Pressure at sea level [pa] 16 | 17 | temp_alt = temp_sealevel - (alt * temp_lapserate); 18 | pressure_alt = pressure_sealevel * ((temp_sealevel/temp_alt)^(-g/(temp_lapserate*287.0))); 19 | rho = pressure_alt / (287.0*temp_alt); 20 | qbar = 0.5 * rho * Vt * Vt; 21 | end -------------------------------------------------------------------------------- /autopilotplant.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sauravag/GPSMATLAB/1549e13ccc5b7efe3d0ec6c3040d421c3b59d811/autopilotplant.mat -------------------------------------------------------------------------------- /body_to_ecef.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Body frame to ECEF coordinate frame conversion % 3 | % Author: Saurav Agarwal % 4 | % Date: January 1, 2011 % 5 | % Dept. of Aerospace Engg., IIT Bombay, Mumbai, India % 6 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 7 | 8 | function [vector_ECEF] = body_to_ecef(pos_body_ecef,phi,theta,psi,vector) 9 | 10 | % C_b2n = [cos(theta)*cos(psi) -cos(phi)*sin(psi)+sin(phi)*sin(theta)*cos(psi) sin(phi)*sin(psi)+cos(phi)*sin(theta)*cos(psi);cos(theta)*sin(psi) cos(phi)*cos(psi)+sin(phi)*sin(theta)*sin(psi) -sin(phi)*cos(psi)+cos(phi)*sin(theta)*sin(psi);-sin(theta) sin(phi)*cos(theta) cos(phi)*cos(theta)]; 11 | 12 | pos_body_geo = ecef_to_latlong(pos_body_ecef); 13 | lat = pos_body_geo.lat; 14 | long = pos_body_geo.long; 15 | C_e2n = [-sin(lat)*cos(long) -sin(lat)*sin(long) cos(lat);-sin(long) cos(long) 0; -cos(lat)*cos(long) -cos(lat)*sin(long) -sin(lat)]; 16 | 17 | vector_ECEF = (C_e2n'*vector')'; 18 | 19 | end -------------------------------------------------------------------------------- /calc_sat_pos_ecef.asv: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Satellite Position/Velocity Calculation Function % 3 | % Author: Saurav Agarwal % 4 | % Date: January 1, 2011 % 5 | % Dept. of Aerospace Engg., IIT Bombay, Mumbai, India % 6 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 7 | % This code calculates satellite position and velocity in ECEF frame from Ephemeris Data % 8 | % method given in Table 3.2 in Chapter 3 in Grewal & Andrews % 9 | % Outputs: 10 | % 1. x_ecef,y_ecef,z_ecef: GPS Satellite Coordinates in ECEF frame 11 | % (m) 12 | % 2. Vecef: Velocity of satellite w.r.t to earth in ECEF frame 13 | % Inputs: 14 | % 1. gps_sat: array containing ephemeris data for individual g 15 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 16 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 17 | 18 | function [x_ecef,y_ecef,z_ecef,Vecef] = calc_sat_pos_ecef(gps_sat,t_sv,sv_id); 19 | 20 | 21 | % Constants 22 | mu = 3.986004418e14; % universal gravitational param (m^3/s^2) 23 | OMEGA_dot_e = 7.292115e-5; % earth's rotation rate (rad/sec) 24 | c = 2.99792458e8; % speed of light (m/s) 25 | F = -2*sqrt(mu)/c^2; % (s/m^1/2) 26 | rtd = 180/3.14159; % radians to degree 27 | dtr = 3.14159/180; 28 | 29 | % Given constant ephemeris data 30 | delta_n = 4.908419e-9; % (rad/s) from grewal 31 | t_oe = 147456.0000; % Ephemeris Reference Time(s) [from the GPS data taken from website] 32 | C_ic = 6.146729e-8; % (rad) 33 | C_rc = 2.259375e2; % (rad) 34 | C_is = 2.086163e-7; % (rad) 35 | C_rs = 7.321875e1; % (rad) 36 | C_uc = 4.017726e-6; % (rad) 37 | C_us = 7.698312e-6; % (rad) 38 | I_dot = 9.178953e-11; % Rate of change of inclination(rad/s) from grewal 39 | 40 | % Time calculations: 41 | % We assume that when transmit time is taken into account, 42 | % t_sv = 248721.9229 This value is used to calculate GPS system 43 | % time below. Note: delta_tr is assumed to be negligible (calculated 44 | % value of delta_tr = 2.6e-8 sec) 45 | 46 | n_0 = sqrt(mu/(gps_sat(sv_id).sqrt_a)^6); % (rad/s) 47 | t_k=t_sv-t_oe; % Time from eph ref epoch (s) 48 | n = n_0 + delta_n; % Corrected mean motion (rad/s) 49 | M_k = gps_sat(sv_id).M_0+n*t_k; % Mean anomaly (rad/s) 50 | 51 | % Perform Newton-Raphson solution for E_k estimate 52 | E_k= newton_raphson(gps_sat(sv_id).e,M_k); % Eccentric anomaly ESTIMATE for computing delta_tr 53 | 54 | num =(sqrt(1-gps_sat(sv_id).e^2)*sin(E_k))/(1-gps_sat(sv_id).e*cos(E_k)); 55 | denom =(cos(E_k)-gps_sat(sv_id).e)/(1-gps_sat(sv_id).e*cos(E_k)); 56 | v_k=atan2(num,denom); % True anom (rad) 57 | E_k=acos((gps_sat(sv_id).e+cos(v_k))/(1+gps_sat(sv_id).e*cos(v_k))); % Eccentric anomaly 58 | PHI_k=v_k+gps_sat(sv_id).omega; % Argument of latitude 59 | 60 | % Second Harmonic Perturbations 61 | deltau_k=C_us*sin(2*PHI_k)+C_uc*cos(2*PHI_k); % Argument of Lat correction 62 | deltar_k=C_rs*sin(2*PHI_k)+C_rc*cos(2*PHI_k); % Radius correction 63 | deltai_k=C_is*sin(2*PHI_k)+C_ic*cos(2*PHI_k); % Inclination correction 64 | 65 | u_k=PHI_k+deltau_k; % Corr. arg of lat 66 | r_k=(gps_sat(sv_id).sqrt_a)^2*(1-gps_sat(sv_id).e*cos(E_k))+deltar_k; % Corrected radius 67 | i_k=gps_sat(sv_id).i_0+deltai_k+I_dot*t_k; % Corrected inclination 68 | 69 | % Positons in orbital plane 70 | xprime_k=r_k*cos(u_k); 71 | yprime_k=r_k*sin(u_k); 72 | 73 | OMEGA_k=gps_sat(sv_id).Omega_0+(gps_sat(sv_id).Omega_dot-OMEGA_dot_e)*t_k-OMEGA_dot_e*t_oe; 74 | 75 | % ECEF coordinates 76 | x_ecef = xprime_k*cos(OMEGA_k)- yprime_k*cos(i_k)*sin(OMEGA_k); 77 | y_ecef = xprime_k*sin(OMEGA_k)+ yprime_k*cos(i_k)*cos(OMEGA_k); 78 | z_ecef = yprime_k*sin(i_k); 79 | 80 | % Velocity Estimation 81 | a = gps_sat(sv_id).sqrt_a^2; 82 | Vorbital = n*a/(1-gps_sat(sv_id).e*cos(E_k))*[-sin(E_k);sqrt(1-gps_sat(sv_id).e^2)*cos(E_k);0];% satellite velocity in orbital plane 83 | 84 | % Formulate Rotation matrix 85 | R1 = [cos(gps_sat(sv_id).omega) -sin(gps_sat(sv_id).omega) 0;sin(gps_sat(sv_id).omega) cos(gps_sat(sv_id).omega) 0;0 0 1]; 86 | R2 = [1 0 0;0 cos(i_k) -sin(i_k);0 sin(i_k) cos(i_k)]; 87 | R3 = [cos(OMEGA_k) -sin(OMEGA_k) 0;sin(OMEGA_k) cos(OMEGA_k) 0;0 0 1]; 88 | R_ORIBT2ECEF= R3*R2*R1; % rotation matrix from orbit to ECEF frame 89 | 90 | Vecef = (R_ORIBT2ECEF*Vorbital - cross([0;0;OMEGA_dot_e],[x_ecef;y_ecef;z_ecef]))'; % Subtracting rotation of earth to get velocity with respect to center of earth in ECEF frame 91 | 92 | 93 | end 94 | 95 | -------------------------------------------------------------------------------- /calc_sat_pos_ecef.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 3 | % Satellite Position/Velocity Calculation Function % 4 | % Author: Saurav Agarwal % 5 | % Date: January 1, 2011 % 6 | % Dept. of Aerospace Engg., IIT Bombay, Mumbai, India % 7 | % All i/o units are specified in brackets % 8 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 9 | % References: 10 | % 1. Grewal & Andrews, Ch 3-Table 3.2 11 | % Outputs: 12 | % 1. x_ecef,y_ecef,z_ecef: GPS Satellite Coordinates in ECEF frame (m) 13 | % 2. Vecef: Velocity of satellite w.r.t to earth in ECEF frame (m/s) 14 | % Inputs: 15 | % 1. gps_sat: array containing ephemeris data of gps satellites 16 | % 2. t_sv: gps time (s) 17 | % 3. sv_id: id number of gps satellite for which to calculate p/v 18 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 19 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 20 | 21 | function [x_ecef,y_ecef,z_ecef,Vecef] = calc_sat_pos_ecef(gps_sat,t_sv,sv_id) 22 | 23 | 24 | % Constants 25 | mu = 3.986004418e14; % universal gravitational param (m^3/s^2) 26 | OMEGA_dot_e = 7.292115e-5; % earth's rotation rate (rad/sec) 27 | c = 2.99792458e8; % speed of light (m/s) 28 | F = -2*sqrt(mu)/c^2; % (s/m^1/2) 29 | rtd = 180/3.14159; % radians to degree 30 | dtr = 3.14159/180; 31 | 32 | % Given constant ephemeris data 33 | delta_n = 4.908419e-9; % (rad/s) from grewal 34 | t_oe = 147456.0000; % Ephemeris Reference Time(s) [from the GPS data taken from website] 35 | C_ic = 6.146729e-8; % (rad) 36 | C_rc = 2.259375e2; % (rad) 37 | C_is = 2.086163e-7; % (rad) 38 | C_rs = 7.321875e1; % (rad) 39 | C_uc = 4.017726e-6; % (rad) 40 | C_us = 7.698312e-6; % (rad) 41 | I_dot = 9.178953e-11; % Rate of change of inclination(rad/s) from grewal 42 | 43 | % Time calculations: 44 | % We assume that when transmit time is taken into account, 45 | % t_sv = 248721.9229 This value is used to calculate GPS system 46 | % time below. Note: delta_tr is assumed to be negligible (calculated 47 | % value of delta_tr = 2.6e-8 sec) 48 | 49 | n_0 = sqrt(mu/(gps_sat(sv_id).sqrt_a)^6); % (rad/s) 50 | t_k=t_sv-t_oe; % Time from eph ref epoch (s) 51 | n = n_0 + delta_n; % Corrected mean motion (rad/s) 52 | M_k = gps_sat(sv_id).M_0+n*t_k; % Mean anomaly (rad/s) 53 | 54 | % Perform Newton-Raphson solution for E_k estimate 55 | E_k= newton_raphson(gps_sat(sv_id).e,M_k); % Eccentric anomaly ESTIMATE for computing delta_tr 56 | 57 | num =(sqrt(1-gps_sat(sv_id).e^2)*sin(E_k))/(1-gps_sat(sv_id).e*cos(E_k)); 58 | denom =(cos(E_k)-gps_sat(sv_id).e)/(1-gps_sat(sv_id).e*cos(E_k)); 59 | v_k=atan2(num,denom); % True anom (rad) 60 | E_k=acos((gps_sat(sv_id).e+cos(v_k))/(1+gps_sat(sv_id).e*cos(v_k))); % Eccentric anomaly 61 | PHI_k=v_k+gps_sat(sv_id).omega; % Argument of latitude 62 | 63 | % Second Harmonic Perturbations 64 | deltau_k=C_us*sin(2*PHI_k)+C_uc*cos(2*PHI_k); % Argument of Lat correction 65 | deltar_k=C_rs*sin(2*PHI_k)+C_rc*cos(2*PHI_k); % Radius correction 66 | deltai_k=C_is*sin(2*PHI_k)+C_ic*cos(2*PHI_k); % Inclination correction 67 | 68 | u_k=PHI_k+deltau_k; % Corr. arg of lat 69 | r_k=(gps_sat(sv_id).sqrt_a)^2*(1-gps_sat(sv_id).e*cos(E_k))+deltar_k; % Corrected radius 70 | i_k=gps_sat(sv_id).i_0+deltai_k+I_dot*t_k; % Corrected inclination 71 | 72 | % Positons in orbital plane 73 | xprime_k=r_k*cos(u_k); 74 | yprime_k=r_k*sin(u_k); 75 | 76 | OMEGA_k=gps_sat(sv_id).Omega_0+(gps_sat(sv_id).Omega_dot-OMEGA_dot_e)*t_k-OMEGA_dot_e*t_oe; 77 | 78 | % ECEF coordinates 79 | x_ecef = xprime_k*cos(OMEGA_k)- yprime_k*cos(i_k)*sin(OMEGA_k); 80 | y_ecef = xprime_k*sin(OMEGA_k)+ yprime_k*cos(i_k)*cos(OMEGA_k); 81 | z_ecef = yprime_k*sin(i_k); 82 | 83 | % Velocity Estimation 84 | a = gps_sat(sv_id).sqrt_a^2; 85 | Vorbital = n*a/(1-gps_sat(sv_id).e*cos(E_k))*[-sin(E_k);sqrt(1-gps_sat(sv_id).e^2)*cos(E_k);0];% satellite velocity in orbital plane 86 | 87 | % Formulate Rotation matrix 88 | R1 = [cos(gps_sat(sv_id).omega) -sin(gps_sat(sv_id).omega) 0;sin(gps_sat(sv_id).omega) cos(gps_sat(sv_id).omega) 0;0 0 1]; 89 | R2 = [1 0 0;0 cos(i_k) -sin(i_k);0 sin(i_k) cos(i_k)]; 90 | R3 = [cos(OMEGA_k) -sin(OMEGA_k) 0;sin(OMEGA_k) cos(OMEGA_k) 0;0 0 1]; 91 | R_ORIBT2ECEF= R3*R2*R1; % rotation matrix from orbit to ECEF frame 92 | 93 | Vecef = (R_ORIBT2ECEF*Vorbital - cross([0;0;OMEGA_dot_e],[x_ecef;y_ecef;z_ecef]))'; % Subtracting rotation of earth to get velocity with respect to center of earth in ECEF frame 94 | 95 | end 96 | -------------------------------------------------------------------------------- /chistart.m: -------------------------------------------------------------------------------- 1 | function Chi2 = chistart (D,L,a,ncands,factor) 2 | %CHISTART: Computes the initial size of the search ellipsoid 3 | % 4 | % This routine computes or approximates the initial size of the search 5 | % ellipsoid. If the requested number of candidates is not more than the 6 | % dimension + 1, this is done by computing the squared distances of partially 7 | % conditionally rounded float vectors to the float vector in the metric of the 8 | % covariance matrix. Otherwise an approximation is used. 9 | % 10 | % Input arguments 11 | % L,D : LtDL-decomposition of the variance-covariance matrix of 12 | % the float ambiguities (preferably decorrelated) 13 | % a : float ambiguites (preferably decorrelated) 14 | % ncands: Requested number of candidates (default = 2) 15 | % factor: Multiplication factor for the volume of the resulting 16 | % search ellipsoid (default = 1.5) 17 | % 18 | % Output arguments: 19 | % Chi2 : Size of the search ellipsoid 20 | 21 | % ---------------------------------------------------------------------- 22 | % File.....: chistart.m 23 | % Date.....: 19-MAY-1999 24 | % Modified.: 05-MAR-2001, by P. Joosten 25 | % Author...: Peter Joosten 26 | % Mathematical Geodesy and Positioning 27 | % Delft University of Technology 28 | % ---------------------------------------------------------------------- 29 | 30 | % ------------------ 31 | % --- Initialize --- 32 | % ------------------ 33 | 34 | if nargin < 4; ncands = 2 ; end; 35 | if nargin < 5; factor = 1.5; end; 36 | 37 | n = max(size(a)); 38 | 39 | % ---------------------------------------------------------------------- 40 | % --- Computation depends on the number of candidates to be computed --- 41 | % ---------------------------------------------------------------------- 42 | 43 | if ncands <= n+1; 44 | 45 | % -------------------------------------------------------- 46 | % --- Computation based on the bootstrapping estimator --- 47 | % -------------------------------------------------------- 48 | 49 | Chi = []; 50 | 51 | for k = n:-1:0; 52 | 53 | afloat = a; 54 | afixed = a; 55 | 56 | for i = n:-1:1; 57 | 58 | dw = 0; 59 | for j = n:-1:i; 60 | dw = dw + L(j,i) * (afloat(j) - afixed(j)); 61 | end; 62 | 63 | afloat(i) = afloat(i) - dw; 64 | if (i ~= k); 65 | afixed(i) = round (afloat(i)); 66 | else; 67 | if isequal (afloat(i),afixed(i)); 68 | afixed(i) = round(afixed(i) + 1); 69 | else; 70 | afixed(i) = round (afloat(i) + sign (afloat(i) - afixed(i))); 71 | end; 72 | end; 73 | 74 | end; 75 | 76 | Chi = [Chi (a-afixed)' * inv(L'*diag(D)*L) * (a-afixed)]; 77 | 78 | end; 79 | 80 | % --------------------------------------------------------------- 81 | % --- Sort the results, and return the appropriate number --- 82 | % --- Add an "eps", to make sure there is no boundary problem --- 83 | % --------------------------------------------------------------- 84 | 85 | Chi = sort(Chi); 86 | Chi2 = Chi(ncands) + 1d-6; 87 | 88 | else 89 | 90 | % ----------------------------------------------------- 91 | % An approximation for the squared norm is computed --- 92 | % ----------------------------------------------------- 93 | 94 | Linv = inv(L); 95 | Dinv = 1./D; 96 | 97 | Vn = (2/n) * (pi ^ (n/2) / gamma(n/2)); 98 | Chi2 = factor * (ncands / sqrt((prod(1 ./ Dinv)) * Vn)) ^ (2/n); 99 | 100 | end; 101 | 102 | % ---------------------------------------------------------------------- 103 | % End of routine: chistart 104 | % ---------------------------------------------------------------------- 105 | -------------------------------------------------------------------------------- /collect_phase_data.asv: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 3 | % GPS Signal Phase Measurement Function % 4 | % Author: Saurav Agarwal % 5 | % Date: January 1, 2011 % 6 | % Dept. of Aerospace Engg., IIT Bombay, Mumbai, India % 7 | % All i/o units are specified in brackets % 8 | % Conventions: 9 | % 1. WGS-84 system used for geodetic model 10 | % 2. Geodetic Coordinates are used in the form lat(rad)/long(rad)/alt(m) 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | % References: 13 | % 1. "Global Positioning System: Theory and Applications Vol II, Bradford W. Parksinson, Pg 435-437" 14 | % Outputs: 15 | % 1. deltaPhi: GPS Satellite Coordinates in ECEF frame (m) 16 | % 2. Scapk: Velocity of satellite w.r.t to earth in ECEF frame (m/s) 17 | % 3. SmoothPhase: 18 | % 4. clbias_u: 19 | % 5. clkbias_r: 20 | % Inputs: 21 | % 1. gps_sat: array containing ephemeris data of gps satellites 22 | % 2. gps_time: gps time (s) 23 | % 3. sv_id: id number of gps satellite for which to calculate p/v 24 | % 4. visible_sats_id: 25 | % 5. ref_station_ecef 26 | % 6. ref_station: reference station pos in 27 | % 7. true_user_pos_geodetic: true user pos in 28 | % 8. true_user_pos_ecef: true ECEF pos (m) 29 | % 9. initial_user_pos_estimate: initial pos estimate in ECEF (m) 30 | % 10. ibeacon1_geo/ibeacon1_ecef: integrity beacon 1 position in geodetic and ECEF (m) coordinates respctly 31 | % 11. ibeacon2_geo/ibeacon2_ecef: integrity beacon 2 position in geodetic and ECEF (m) coordinates respctly 32 | % lat(rad)/long(rad)/alt(m) and ECEF (m) coordinates respctly. 33 | % 12. initial_bias_u/initial_bias_r: initial clock bias of user and 34 | % reference reciever respctly. 35 | 36 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 37 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 38 | % Author: Saurav Agarwal 39 | % For Carrier Phase Differential GPS% 40 | % Ref. 41 | 42 | 43 | function [deltaPhi,Scapk,SmoothPhase,clkbias_u,clkbias_r] = collect_phase_data(gps_sat,gps_time,visible_sats_id,ref_station_ecef,ref_station,true_user_pos_geodetic,true_user_pos_ecef,initial_user_pos_estimate,ibeacon1_geo,ibeacon1_ecef,ibeacon2_geo,ibeacon2_ecef,initial_bias_u,initial_bias_r) 44 | 45 | c = 2.99792458e8;% speed of light (m/s) 46 | f_L1 = 1575.42e6; %frequency of L1 carrier wave Hz 47 | xu = initial_user_pos_estimate.x; 48 | yu = initial_user_pos_estimate.y; 49 | zu = initial_user_pos_estimate.z; 50 | 51 | xutrue = true_user_pos_ecef.x; 52 | yutrue = true_user_pos_ecef.y; 53 | zutrue = true_user_pos_ecef.z; 54 | 55 | user_ecef_pos_estimate = struct('x',xu,'y',yu,'z',zu); 56 | 57 | % coordinates of reference station & integrity beacons 58 | xr = ref_station_ecef.x; 59 | yr = ref_station_ecef.y; 60 | zr = ref_station_ecef.z; 61 | 62 | xib1 = ibeacon1_ecef.x; 63 | yib1 = ibeacon1_ecef.y; 64 | zib1 = ibeacon1_ecef.z; 65 | 66 | xib2 = ibeacon2_ecef.x; 67 | yib2 = ibeacon2_ecef.y; 68 | zib2 = ibeacon2_ecef.z; 69 | 70 | dim = length(visible_sats_id); 71 | 72 | lambda_L1 = 0.19; % metres 73 | 74 | range_ru_true = compute_distance(true_user_pos_ecef,ref_station_ecef); % the true range between the reference station and satellite 75 | range_uib1_true = compute_distance(true_user_pos_ecef,ibeacon1_ecef); 76 | range_uib2_true = compute_distance(true_user_pos_ecef, ibeacon2_ecef); 77 | range_rib1_true = compute_distance(ref_station_ecef,ibeacon1_ecef); 78 | range_rib2_true = compute_distance(ref_station_ecef, ibeacon2_ecef); 79 | 80 | x = [xu-xr;yu-yr;zu-zr]; % relative vector from ref to user 81 | 82 | randomfactor_u = randn; % gaussian noise factor in user clock 83 | randomfactor_r = randn; % gaussian noise factor in reference clock 84 | randomfactor_ib1 = randn; % gaussian noise factor in user clock 85 | randomfactor_ib2 = randn; % gaussian noise factor in reference clock 86 | 87 | clkbias_u = rcvr_clk_model(initial_bias_u,randn); 88 | 89 | clkbias_r = rcvr_clk_model(initial_bias_r,randn); 90 | 91 | for k = 1:dim 92 | 93 | sv_id = visible_sats_id(k); 94 | 95 | [xs(k),ys(k),zs(k)] = calc_sat_pos_ecef(gps_sat,gps_time,sv_id); % The true position of satellite based on ephemeris data and the time embedded in message 96 | 97 | true_sat_pos_ecef = struct('x',xs(k),'y',ys(k),'z',zs(k)); 98 | 99 | r_rs(k) = compute_distance(true_sat_pos_ecef,ref_station_ecef); % the true range between the reference station and satellite 100 | 101 | r_us(k) = compute_distance(true_sat_pos_ecef,true_user_pos_ecef); 102 | 103 | N = k; 104 | if N >3 105 | N = k - 3; 106 | end; 107 | if N > 6 108 | N = k - 3; 109 | end; 110 | if N==1 111 | N = 0; 112 | end; 113 | 114 | trueambiguity(k) = (-1)^k*N*1057; 115 | 116 | s = [(xs(k)-xr);(ys(k)-yr);(zs(k)-zr)]/r_rs(k);%[(xs(k)-(xu+xr)/2);(ys(k)-(yu+yr)/2);(zs(k)-(zu+zr)/2)]/r_rs(k) ; % the line of sight vector from ref station to satelite k 117 | 118 | psi_ru(k) = r_us(k) - r_rs(k) + (-1)^k*N*1057*lambda_L1 + (clkbias_u -clkbias_r)*c + randn/100; % the single difference between the phase at reference station and user for satellite k 119 | 120 | [pr_measured_L1_u, pr_measured_L2, pr_measured_L5] = eval_pr_measurement(gps_sat,sv_id,gps_time,true_user_pos_ecef,clkbias_u,true_sat_pos_ecef); 121 | 122 | [pr_measured_L1_r, pr_measured_L2, pr_measured_L5] = eval_pr_measurement(gps_sat,sv_id,gps_time,ref_station_ecef,clkbias_r,true_sat_pos_ecef); 123 | 124 | rho_ru(k) = pr_measured_L1_u - pr_measured_L1_r ; % single difference code measurement 125 | 126 | delta_psi_ru(k) = psi_ru(k)+ s'*x; 127 | 128 | Scap(k,:) = [-s' 1]; 129 | end; 130 | 131 | e1 = [xib1-xu;yib1-yu;zib1-zu]/norm([xib1-xu;yib1-yu;zib1-zu]); 132 | e2 = [xib2-xu;yib2-yu;zib2-zu]/norm([xib2-xu;yib2-yu;zib2-zu]); 133 | 134 | e1dash = [-e1' 1]; 135 | e2dash = [-e2' 1]; 136 | 137 | Scapk = [Scap;e1dash;e2dash]; 138 | 139 | p1 = [xib1-xr;yib1-yr;zib1-zr];% rel vector from ref to ib1 140 | p2 = [xib2-xr;yib2-yr;zib2-zr];% rel vector from ref to ib2 141 | 142 | phi_ruib1 = (range_uib1_true-range_rib1_true) + 3100*lambda_L1 + (clkbias_u -clkbias_r)*c + randn/100; %the single difference between the phase at user and ref station for integrity beacon 1 143 | 144 | rcvr_noise = randn/2; % Receiver error due to measurement noise 145 | 146 | if rcvr_noise > 0.5 147 | rcvr_noise = 0.5; 148 | end; 149 | if rcvr_noise <-0.5 150 | rcvr_noise = -0.5; 151 | end; 152 | 153 | rho_ruib1 = (range_uib1_true-range_rib1_true) + (clkbias_u -clkbias_r)*c + rcvr_noise ; 154 | 155 | phi_ruib2 = (range_uib2_true-range_rib2_true) + 2200*lambda_L1 + (clkbias_u -clkbias_r)*c + randn/100; %the single difference between the phase at user and ref station for integrity beacon 2 156 | rcvr_noise = randn/2; % Receiver error due to measurement noise 157 | 158 | if rcvr_noise > 0.5 159 | rcvr_noise = 0.5; 160 | end; 161 | if rcvr_noise <-0.5 162 | rcvr_noise = -0.5; 163 | end; 164 | rho_ruib2 = (range_uib2_true-range_rib2_true) + (clkbias_u -clkbias_r)*c + rcvr_noise; 165 | 166 | delta_phi_ruib1 = phi_ruib1 - (norm(p1-x) - norm(p1)); 167 | delta_phi_ruib2 = phi_ruib2 - (norm(p2-x) - norm(p2)); 168 | 169 | deltaPhi = [delta_psi_ru';delta_phi_ruib1;delta_phi_ruib2]; 170 | SDPhi = [psi_ru';phi_ruib1;phi_ruib2]; 171 | SDRho = [rho_ru';rho_ruib1;rho_ruib2]; 172 | SmoothPhase = SDPhi - SDRho; 173 | actualambiguity = [trueambiguity';3100;2200]; 174 | end -------------------------------------------------------------------------------- /collect_phase_data.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 3 | % GPS Signal Phase Measurement Function % 4 | % Author: Saurav Agarwal % 5 | % Date: January 1, 2011 % 6 | % Dept. of Aerospace Engg., IIT Bombay, Mumbai, India % 7 | % All i/o units are specified in brackets % 8 | % Conventions: 9 | % 1. WGS-84 system used for geodetic model 10 | % 2. Geodetic Coordinates are used in the form lat(rad)/long(rad)/alt(m) 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | % References: 13 | % 1. "Global Positioning System: Theory and Applications Vol II, Bradford W. Parksinson, Pg 435-437" 14 | % Outputs: 15 | % 1. deltaPhi: single difference phase measurements between user and ref 16 | % 2. Scapk: geometry matrix 17 | % 3. SmoothPhase: smoothed pseudorange 18 | % 4. clbias_u: user clock bias (m) 19 | % 5. clkbias_r: reference clock bias (m) 20 | % Inputs: 21 | % 1. gps_sat: array containing ephemeris data of gps satellites 22 | % 2. gps_time: gps time (s) 23 | % 3. sv_id: id number of gps satellite for which to calculate p/v 24 | % 4. visible_sats_id: 25 | % 5. ref_station_ecef 26 | % 6. ref_station: reference station pos in 27 | % 7. true_user_pos_geodetic: true user pos in 28 | % 8. true_user_pos_ecef: true ECEF pos (m) 29 | % 9. initial_user_pos_estimate: initial pos estimate in ECEF (m) 30 | % 10. ibeacon1_geo/ibeacon1_ecef: integrity beacon 1 position in geodetic and ECEF (m) coordinates respctly 31 | % 11. ibeacon2_geo/ibeacon2_ecef: integrity beacon 2 position in geodetic and ECEF (m) coordinates respctly 32 | % 12. initial_bias_u/initial_bias_r: initial clock bias of user and reference reciever respctly (s) 33 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 34 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 35 | 36 | function [deltaPhi,Scapk,SmoothPhase,clkbias_u,clkbias_r] = collect_phase_data(gps_sat,gps_time,visible_sats_id,ref_station_ecef,ref_station,true_user_pos_geodetic,true_user_pos_ecef,initial_user_pos_estimate,ibeacon1_geo,ibeacon1_ecef,ibeacon2_geo,ibeacon2_ecef,initial_bias_u,initial_bias_r) 37 | 38 | c = 2.99792458e8;% speed of light (m/s) 39 | f_L1 = 1575.42e6; %frequency of L1 carrier wave Hz 40 | xu = initial_user_pos_estimate.x; 41 | yu = initial_user_pos_estimate.y; 42 | zu = initial_user_pos_estimate.z; 43 | 44 | xutrue = true_user_pos_ecef.x; 45 | yutrue = true_user_pos_ecef.y; 46 | zutrue = true_user_pos_ecef.z; 47 | 48 | user_ecef_pos_estimate = struct('x',xu,'y',yu,'z',zu); 49 | 50 | % coordinates of reference station & integrity beacons 51 | xr = ref_station_ecef.x; 52 | yr = ref_station_ecef.y; 53 | zr = ref_station_ecef.z; 54 | 55 | xib1 = ibeacon1_ecef.x; 56 | yib1 = ibeacon1_ecef.y; 57 | zib1 = ibeacon1_ecef.z; 58 | 59 | xib2 = ibeacon2_ecef.x; 60 | yib2 = ibeacon2_ecef.y; 61 | zib2 = ibeacon2_ecef.z; 62 | 63 | dim = length(visible_sats_id); 64 | 65 | lambda_L1 = 0.19; % metres 66 | 67 | range_ru_true = compute_distance(true_user_pos_ecef,ref_station_ecef); % the true range between the reference station and satellite 68 | range_uib1_true = compute_distance(true_user_pos_ecef,ibeacon1_ecef); 69 | range_uib2_true = compute_distance(true_user_pos_ecef, ibeacon2_ecef); 70 | range_rib1_true = compute_distance(ref_station_ecef,ibeacon1_ecef); 71 | range_rib2_true = compute_distance(ref_station_ecef, ibeacon2_ecef); 72 | 73 | x = [xu-xr;yu-yr;zu-zr]; % relative vector from ref to user 74 | 75 | randomfactor_u = randn; % gaussian noise factor in user clock 76 | randomfactor_r = randn; % gaussian noise factor in reference clock 77 | randomfactor_ib1 = randn; % gaussian noise factor in user clock 78 | randomfactor_ib2 = randn; % gaussian noise factor in reference clock 79 | 80 | clkbias_u = rcvr_clk_model(initial_bias_u,randn); 81 | 82 | clkbias_r = rcvr_clk_model(initial_bias_r,randn); 83 | 84 | for k = 1:dim 85 | 86 | sv_id = visible_sats_id(k); 87 | 88 | [xs(k),ys(k),zs(k)] = calc_sat_pos_ecef(gps_sat,gps_time,sv_id); % The true position of satellite based on ephemeris data and the time embedded in message 89 | 90 | true_sat_pos_ecef = struct('x',xs(k),'y',ys(k),'z',zs(k)); 91 | 92 | r_rs(k) = compute_distance(true_sat_pos_ecef,ref_station_ecef); % the true range between the reference station and satellite 93 | 94 | r_us(k) = compute_distance(true_sat_pos_ecef,true_user_pos_ecef); 95 | 96 | N = k; 97 | if N >3 98 | N = k - 3; 99 | end; 100 | if N > 6 101 | N = k - 3; 102 | end; 103 | if N==1 104 | N = 0; 105 | end; 106 | 107 | trueambiguity(k) = (-1)^k*N*1057; 108 | 109 | s = [(xs(k)-xr);(ys(k)-yr);(zs(k)-zr)]/r_rs(k);%[(xs(k)-(xu+xr)/2);(ys(k)-(yu+yr)/2);(zs(k)-(zu+zr)/2)]/r_rs(k) ; % the line of sight vector from ref station to satelite k 110 | 111 | psi_ru(k) = r_us(k) - r_rs(k) + (-1)^k*N*1057*lambda_L1 + (clkbias_u -clkbias_r)*c + randn/100; % the single difference between the phase at reference station and user for satellite k 112 | 113 | [pr_measured_L1_u, pr_measured_L2, pr_measured_L5] = eval_pr_measurement(gps_sat,sv_id,gps_time,true_user_pos_ecef,clkbias_u,true_sat_pos_ecef); 114 | 115 | [pr_measured_L1_r, pr_measured_L2, pr_measured_L5] = eval_pr_measurement(gps_sat,sv_id,gps_time,ref_station_ecef,clkbias_r,true_sat_pos_ecef); 116 | 117 | rho_ru(k) = pr_measured_L1_u - pr_measured_L1_r ; % single difference code measurement 118 | 119 | delta_psi_ru(k) = psi_ru(k)+ s'*x; 120 | 121 | Scap(k,:) = [-s' 1]; 122 | end; 123 | 124 | e1 = [xib1-xu;yib1-yu;zib1-zu]/norm([xib1-xu;yib1-yu;zib1-zu]); 125 | e2 = [xib2-xu;yib2-yu;zib2-zu]/norm([xib2-xu;yib2-yu;zib2-zu]); 126 | 127 | e1dash = [-e1' 1]; 128 | e2dash = [-e2' 1]; 129 | 130 | Scapk = [Scap;e1dash;e2dash]; 131 | 132 | p1 = [xib1-xr;yib1-yr;zib1-zr];% rel vector from ref to ib1 133 | p2 = [xib2-xr;yib2-yr;zib2-zr];% rel vector from ref to ib2 134 | 135 | phi_ruib1 = (range_uib1_true-range_rib1_true) + 3100*lambda_L1 + (clkbias_u -clkbias_r)*c + randn/100; %the single difference between the phase at user and ref station for integrity beacon 1 136 | 137 | rcvr_noise = randn/2; % Receiver error due to measurement noise 138 | 139 | if rcvr_noise > 0.5 140 | rcvr_noise = 0.5; 141 | end; 142 | if rcvr_noise <-0.5 143 | rcvr_noise = -0.5; 144 | end; 145 | 146 | rho_ruib1 = (range_uib1_true-range_rib1_true) + (clkbias_u -clkbias_r)*c + rcvr_noise ; 147 | 148 | phi_ruib2 = (range_uib2_true-range_rib2_true) + 2200*lambda_L1 + (clkbias_u -clkbias_r)*c + randn/100; %the single difference between the phase at user and ref station for integrity beacon 2 149 | rcvr_noise = randn/2; % Receiver error due to measurement noise 150 | 151 | if rcvr_noise > 0.5 152 | rcvr_noise = 0.5; 153 | end; 154 | if rcvr_noise <-0.5 155 | rcvr_noise = -0.5; 156 | end; 157 | rho_ruib2 = (range_uib2_true-range_rib2_true) + (clkbias_u -clkbias_r)*c + rcvr_noise; 158 | 159 | delta_phi_ruib1 = phi_ruib1 - (norm(p1-x) - norm(p1)); 160 | delta_phi_ruib2 = phi_ruib2 - (norm(p2-x) - norm(p2)); 161 | 162 | deltaPhi = [delta_psi_ru';delta_phi_ruib1;delta_phi_ruib2]; 163 | SDPhi = [psi_ru';phi_ruib1;phi_ruib2]; 164 | SDRho = [rho_ru';rho_ruib1;rho_ruib2]; 165 | SmoothPhase = SDPhi - SDRho; 166 | actualambiguity = [trueambiguity';3100;2200]; 167 | end -------------------------------------------------------------------------------- /collect_phase_data2.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 3 | % GPS Signal Phase Measurement Function % 4 | % Author: Saurav Agarwal 5 | % Email: saurav6@gmail.com 6 | % Date: January 1, 2011 7 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 8 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 9 | % All i/o units are specified in brackets % 10 | % Conventions: 11 | % 1. WGS-84 system used for geodetic model 12 | % 2. Geodetic Coordinates are used in the form lat(rad)/long(rad)/alt(m) 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | % References: 15 | % 1. "Global Positioning System: Theory and Applications Vol II, Bradford W. Parksinson, Pg 435-437" 16 | % Outputs: 17 | % 1. deltaPhi: single difference phase measurements between user and ref 18 | % 2. Scapk: geometry matrix 19 | % 3. SmoothPhase: smoothed pseudorange 20 | % 4. clbias_u: user clock bias (m) 21 | % 5. clkbias_r: reference clock bias (m) 22 | % Inputs: 23 | % 1. gps_sat: array containing ephemeris data of gps satellites 24 | % 2. gps_time: gps time (s) 25 | % 3. sv_id: id number of gps satellite for which to calculate p/v 26 | % 4. visible_sats_id: 27 | % 5. ref_station_ecef 28 | % 6. ref_station: reference station pos in 29 | % 7. true_user_pos_geodetic: true user pos in 30 | % 8. true_user_pos_ecef: true ECEF pos (m) 31 | % 9. initial_user_pos_estimate: initial pos estimate in ECEF (m) 32 | % 10. ibeacon1_geo/ibeacon1_ecef: integrity beacon 1 position in geodetic and ECEF (m) coordinates respctly 33 | % 11. ibeacon2_geo/ibeacon2_ecef: integrity beacon 2 position in geodetic and ECEF (m) coordinates respctly 34 | % 12. initial_bias_u/initial_bias_r: initial clock bias of user and reference reciever respctly (s) 35 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 36 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 37 | function [deltaPhi,Scapk,SmoothPhase] = collect_phase_data2(gps_sat,gps_time,visible_sats_id,ref_station_ecef,ref_station,true_user_pos_geodetic,true_user_pos_ecef,initial_user_pos_estimate,ibeacon1_geo,ibeacon1_ecef,ibeacon2_geo,ibeacon2_ecef,clkbias_u,clkbias_r) 38 | 39 | c = 2.99792458e8;% speed of light (m/s) 40 | f_L1 = 1575.42e6; %frequency of L1 carrier wave Hz 41 | xu = initial_user_pos_estimate.x; 42 | yu = initial_user_pos_estimate.y; 43 | zu = initial_user_pos_estimate.z; 44 | 45 | xutrue = true_user_pos_ecef.x; 46 | yutrue = true_user_pos_ecef.y; 47 | zutrue = true_user_pos_ecef.z; 48 | 49 | user_ecef_pos_estimate = struct('x',xu,'y',yu,'z',zu); 50 | 51 | % coordinates of reference station & integrity beacons 52 | xr = ref_station_ecef.x; 53 | yr = ref_station_ecef.y; 54 | zr = ref_station_ecef.z; 55 | 56 | xib1 = ibeacon1_ecef.x; 57 | yib1 = ibeacon1_ecef.y; 58 | zib1 = ibeacon1_ecef.z; 59 | 60 | xib2 = ibeacon2_ecef.x; 61 | yib2 = ibeacon2_ecef.y; 62 | zib2 = ibeacon2_ecef.z; 63 | 64 | dim = length(visible_sats_id); 65 | 66 | lambda_L1 = 0.19; % metres 67 | 68 | range_ru_true = compute_distance(true_user_pos_ecef,ref_station_ecef); % the true range between the reference station and satellite 69 | range_uib1_true = compute_distance(true_user_pos_ecef,ibeacon1_ecef); 70 | range_uib2_true = compute_distance(true_user_pos_ecef, ibeacon2_ecef); 71 | range_rib1_true = compute_distance(ref_station_ecef,ibeacon1_ecef); 72 | range_rib2_true = compute_distance(ref_station_ecef, ibeacon2_ecef); 73 | 74 | x = [xu-xr;yu-yr;zu-zr]; % relative vector from ref to user 75 | 76 | randomfactor_u = randn; % gaussian noise factor in user clock 77 | randomfactor_r = randn; % gaussian noise factor in reference clock 78 | randomfactor_ib1 = randn; % gaussian noise factor in user clock 79 | randomfactor_ib2 = randn; % gaussian noise factor in reference clock 80 | 81 | for k = 1:dim 82 | 83 | sv_id = visible_sats_id(k); 84 | 85 | [xs(k),ys(k),zs(k)] = calc_sat_pos_ecef(gps_sat,gps_time,sv_id); % The true position of satellite based on ephemeris data and the time embedded in message 86 | 87 | true_sat_pos_ecef = struct('x',xs(k),'y',ys(k),'z',zs(k)); 88 | 89 | r_rs(k) = compute_distance(true_sat_pos_ecef,ref_station_ecef); % the true range between the reference station and satellite 90 | 91 | r_us(k) = compute_distance(true_sat_pos_ecef,true_user_pos_ecef); 92 | 93 | N = k; 94 | if N >3 95 | N = k - 3; 96 | end; 97 | if N > 6 98 | N = k - 3; 99 | end; 100 | if N==1 101 | N = 0; 102 | end; 103 | 104 | trueambiguity(k) = (-1)^k*N*1057; 105 | 106 | s = [(xs(k)-xr);(ys(k)-yr);(zs(k)-zr)]/r_rs(k);%[(xs(k)-(xu+xr)/2);(ys(k)-(yu+yr)/2);(zs(k)-(zu+zr)/2)]/r_rs(k) ; % the line of sight vector from ref station to satelite k 107 | 108 | psi_ru(k) = r_us(k) - r_rs(k) + (-1)^k*N*1057*lambda_L1 + (clkbias_u -clkbias_r)*c + randn/100; % the single difference between the phase at reference station and user for satellite k 109 | 110 | [pr_measured_L1_u, pr_measured_L2, pr_measured_L5] = eval_pr_measurement(gps_sat,sv_id,gps_time,true_user_pos_ecef,clkbias_u,true_sat_pos_ecef); 111 | 112 | [pr_measured_L1_r, pr_measured_L2, pr_measured_L5] = eval_pr_measurement(gps_sat,sv_id,gps_time,ref_station_ecef,clkbias_r,true_sat_pos_ecef); 113 | 114 | rho_ru(k) = pr_measured_L1_u - pr_measured_L1_r ; % single difference code measurement 115 | 116 | delta_psi_ru(k) = psi_ru(k)+ s'*x; 117 | 118 | Scap(k,:) = [-s' 1]; 119 | end; 120 | 121 | e1 = [xib1-xu;yib1-yu;zib1-zu]/norm([xib1-xu;yib1-yu;zib1-zu]); 122 | e2 = [xib2-xu;yib2-yu;zib2-zu]/norm([xib2-xu;yib2-yu;zib2-zu]); 123 | 124 | e1dash = [-e1' 1]; 125 | e2dash = [-e2' 1]; 126 | 127 | Scapk = [Scap;e1dash;e2dash]; 128 | 129 | p1 = [xib1-xr;yib1-yr;zib1-zr];% rel vector from ref to ib1 130 | p2 = [xib2-xr;yib2-yr;zib2-zr];% rel vector from ref to ib2 131 | 132 | phi_ruib1 = (range_uib1_true-range_rib1_true) + 3100*lambda_L1 + (clkbias_u -clkbias_r)*c + randn/100; %the single difference between the phase at user and ref station for integrity beacon 1 133 | 134 | rcvr_noise = randn/2; % Receiver error due to measurement noise 135 | 136 | if rcvr_noise > 0.5 137 | rcvr_noise = 0.5; 138 | end; 139 | if rcvr_noise <-0.5 140 | rcvr_noise = -0.5; 141 | end; 142 | 143 | rho_ruib1 = (range_uib1_true-range_rib1_true) + (clkbias_u -clkbias_r)*c + rcvr_noise ; 144 | 145 | phi_ruib2 = (range_uib2_true-range_rib2_true) + 2200*lambda_L1 + (clkbias_u -clkbias_r)*c + randn/100; %the single difference between the phase at user and ref station for integrity beacon 2 146 | rcvr_noise = randn/2; % Receiver error due to measurement noise 147 | 148 | if rcvr_noise > 0.5 149 | rcvr_noise = 0.5; 150 | end; 151 | if rcvr_noise <-0.5 152 | rcvr_noise = -0.5; 153 | end; 154 | rho_ruib2 = (range_uib2_true-range_rib2_true) + (clkbias_u -clkbias_r)*c + rcvr_noise; 155 | 156 | delta_phi_ruib1 = phi_ruib1 - (norm(p1-x) - norm(p1)); 157 | delta_phi_ruib2 = phi_ruib2 - (norm(p2-x) - norm(p2)); 158 | 159 | deltaPhi = [delta_psi_ru';delta_phi_ruib1;delta_phi_ruib2]; 160 | SDPhi = [psi_ru';phi_ruib1;phi_ruib2]; 161 | SDRho = [rho_ru';rho_ruib1;rho_ruib2]; 162 | SmoothPhase = SDPhi - SDRho; 163 | actualambiguity = [trueambiguity';3100;2200]; 164 | end -------------------------------------------------------------------------------- /compute_distance.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 3 | % Distance Computation Function % 4 | % Author: Saurav Agarwal % 5 | % Date: January 1, 2011 % 6 | % Dept. of Aerospace Engg., IIT Bombay, Mumbai, India % 7 | % All i/o units are specified in brackets % 8 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 9 | % Outputs: 10 | % 1. distance: (m) 11 | % Inputs: 12 | % 1. pos1/pos2: pos of 2 points in ECEF 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | function [distance] = compute_distance(pos1,pos2); 15 | 16 | distance = sqrt((pos1.x-pos2.x)^2 + (pos1.y-pos2.y)^2+(pos1.z-pos2.z)^2 ); 17 | 18 | end -------------------------------------------------------------------------------- /ecef_to_latlong.asv: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sauravag/GPSMATLAB/1549e13ccc5b7efe3d0ec6c3040d421c3b59d811/ecef_to_latlong.asv -------------------------------------------------------------------------------- /ecef_to_latlong.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sauravag/GPSMATLAB/1549e13ccc5b7efe3d0ec6c3040d421c3b59d811/ecef_to_latlong.m -------------------------------------------------------------------------------- /enu_to_ecef.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % ENU to ECEF coordinate frame conversion function 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | function [pos_ecef] = enu_to_ecef(ac_pos_enu, C_ne, ref_frame_pos_ecef) 9 | 10 | position_ecef = [ref_frame_pos_ecef.x;ref_frame_pos_ecef.y;ref_frame_pos_ecef.z;] + C_ne*[ac_pos_enu.x; ac_pos_enu.y; ac_pos_enu.z]; 11 | 12 | x = position_ecef(1); 13 | y = position_ecef(2); 14 | z = position_ecef(3); 15 | 16 | pos_ecef = struct('x',x,'y',y,'z',z); 17 | 18 | end 19 | 20 | 21 | -------------------------------------------------------------------------------- /estimate_user_position_cdgps2.asv: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Estimates the user position using Carrier Phase D-GPS measurements % 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | % This function estimates the user position using Carrier Phase D-GPS measurements % 9 | % Author: Saurav Agarwal 10 | % For Carrier Phase Differential GPS% 11 | % Ref. "Global Positioning System: Theory and Applications Vol II, Bradford W. Parksinson, Pg 435" 12 | % Ref. " Global Positioning Systems: Signals, Measurements & Performance, Mishra & Enge, Ch 7" 13 | 14 | function [user_pos_cdgps2,clk_bias_u,clk_bias_r] = estimate_user_position_cdgps2(IntegerAmbiguities,gps_sat,gps_time,visible_sats_id,ref_station_ecef,ref_station,true_user_pos_geodetic,true_user_pos_ecef, initial_user_pos_estimate, ibeacon1_geo,ibeacon1_ecef,ibeacon2_geo,ibeacon2_ecef,initial_clk_bias_u,initial_clk_bias_r); 15 | 16 | xu = initial_user_pos_estimate.x; 17 | yu = initial_user_pos_estimate.y; 18 | zu = initial_user_pos_estimate.z; 19 | 20 | deltax = 1; 21 | deltay = 1; 22 | deltaz = 1; 23 | 24 | clkbias_u = rcvr_clk_model(initial_clk_bias_u,randn); 25 | 26 | clkbias_r = rcvr_clk_model(initial_clk_bias_r,randn); 27 | 28 | while abs(deltax)>1e-2 && abs(deltay)>1e-2 && abs(deltaz) > 1e-2 29 | 30 | [deltaPhi,Scapk] = collect_phase_data2(gps_sat,gps_time,visible_sats_id,ref_station_ecef,ref_station,true_user_pos_geodetic,true_user_pos_ecef, initial_user_pos_estimate, ibeacon1_geo,ibeacon1_ecef,ibeacon2_geo,ibeacon2_ecef,clkbias_u,clkbias_r); 31 | 32 | delta_x = (Scapk'*Scapk)^-1*Scapk'*(deltaPhi - 0.19*IntegerAmbiguities'); 33 | 34 | deltax = delta_x(1); 35 | deltay = delta_x(2); 36 | deltaz = delta_x(3); 37 | 38 | xu = xu + deltax; 39 | yu = yu + deltay; 40 | zu = zu + deltaz; 41 | 42 | initial_user_pos_estimate = struct('x',xu,'y',yu,'z',zu); 43 | 44 | user_pos_cdgps2 = initial_user_pos_estimate; 45 | 46 | end; 47 | 48 | end 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /estimate_user_position_cdgps2.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Estimates the user position using Carrier Phase D-GPS measurements % 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | % This function estimates the user position using Carrier Phase D-GPS measurements % 9 | % Author: Saurav Agarwal 10 | % For Carrier Phase Differential GPS% 11 | % Ref. "Global Positioning System: Theory and Applications Vol II, Bradford W. Parksinson, Pg 435" 12 | % Ref. " Global Positioning Systems: Signals, Measurements & Performance, Mishra & Enge, Ch 7" 13 | 14 | function [user_pos_cdgps2,clk_bias_u,clk_bias_r] = estimate_user_position_cdgps2(IntegerAmbiguities,gps_sat,gps_time,visible_sats_id,ref_station_ecef,ref_station,true_user_pos_geodetic,true_user_pos_ecef, initial_user_pos_estimate, ibeacon1_geo,ibeacon1_ecef,ibeacon2_geo,ibeacon2_ecef,initial_clk_bias_u,initial_clk_bias_r); 15 | 16 | xu = initial_user_pos_estimate.x; 17 | yu = initial_user_pos_estimate.y; 18 | zu = initial_user_pos_estimate.z; 19 | 20 | deltax = 1; 21 | deltay = 1; 22 | deltaz = 1; 23 | 24 | clkbias_u = rcvr_clk_model(initial_clk_bias_u,randn); 25 | 26 | clkbias_r = rcvr_clk_model(initial_clk_bias_r,randn); 27 | 28 | while abs(deltax)>1e-2 && abs(deltay)>1e-2 && abs(deltaz) > 1e-2 29 | 30 | [deltaPhi,Scapk] = collect_phase_data2(gps_sat,gps_time,visible_sats_id,ref_station_ecef,ref_station,true_user_pos_geodetic,true_user_pos_ecef, initial_user_pos_estimate, ibeacon1_geo,ibeacon1_ecef,ibeacon2_geo,ibeacon2_ecef,clkbias_u,clkbias_r); 31 | 32 | delta_x = (Scapk'*Scapk)^-1*Scapk'*(deltaPhi - 0.19*IntegerAmbiguities'); 33 | 34 | deltax = delta_x(1); 35 | deltay = delta_x(2); 36 | deltaz = delta_x(3); 37 | 38 | xu = xu + deltax; 39 | yu = yu + deltay; 40 | zu = zu + deltaz; 41 | 42 | initial_user_pos_estimate = struct('x',xu,'y',yu,'z',zu); 43 | 44 | user_pos_cdgps2 = initial_user_pos_estimate; 45 | 46 | end; 47 | 48 | end 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /eval_DOP.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Dilution of Precision Calculation 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | function [GDOP, PDOP, HDOP, VDOP] = eval_DOP(A) 9 | 10 | Q = (A'*A)^-1; 11 | 12 | GDOP = sqrt(trace(Q)); 13 | PDOP = sqrt(trace(Q)-Q(4,4)); 14 | HDOP = sqrt(Q(1,1)+Q(2,2)); 15 | VDOP = sqrt(Q(1,1)); 16 | end -------------------------------------------------------------------------------- /eval_Thrust.m: -------------------------------------------------------------------------------- 1 | % FUNCTION TO CALCULATE THRUST 2 | function [Thrust] = eval_Thrust(rho,Vt,dth); 3 | Pi = 3.141592654; 4 | J = Vt / (dth * 0.3); 5 | Thrust = 33 * (4/(Pi^2)) * rho * (dth ^ 2) * (0.3^4) * (-0.0948 * (J^2) + 0.058 * J + 0.0761 ); 6 | end -------------------------------------------------------------------------------- /eval_aerod_coeff.asv: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Aerodynamic Coefficient Calculation 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 9 | % References: 10 | % 1. "Aircraft Control and Simulation", B.L Stevenson & Frank M. Lewis 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | % Outputs: 13 | % 1. deltaPhi: single difference phase measurements between user and ref 14 | % 2. Scapk: geometry matrix 15 | % 3. SmoothPhase: smoothed pseudorange 16 | % 4. clbias_u: user clock bias (m) 17 | % 5. clkbias_r: reference clock bias (m) 18 | % Inputs: 19 | % 1. gps_sat: array containing ephemeris data of gps satellites 20 | % 2. gps_time: gps time (s) 21 | % 3. sv_id: id number of gps satellite for which to calculate p/v 22 | % 4. visible_sats_id: 23 | % 5. ref_station_ecef 24 | % 6. ref_station: reference station pos in 25 | % 7. true_user_pos_geodetic: true user pos in 26 | % 8. true_user_pos_ecef: true ECEF pos (m) 27 | % 9. initial_user_pos_estimate: initial pos estimate in ECEF (m) 28 | % 10. ibeacon1_geo/ibeacon1_ecef: integrity beacon 1 position in geodetic and ECEF (m) coordinates respctly 29 | % 11. ibeacon2_geo/ibeacon2_ecef: integrity beacon 2 position in geodetic and ECEF (m) coordinates respctly 30 | % 12. initial_bias_u/initial_bias_r: initial clock bias of user and reference reciever respctly (s) 31 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 32 | function [CD_fa,CD_fade,CL_fa,CL_fade,CY_fbdr,Cl_fada,Cm_fa,Cm_fade,Cn_fbdr,Cn_fada] = eval_aerod_coeff(de,da,dr,ALPHA,BETA); 33 | CD_fa = eval_CD_fa(ALPHA); 34 | CD_fade = eval_CD_fade(de,ALPHA); 35 | CL_fa = eval_CL_fa(ALPHA); 36 | CL_fade = eval_CL_fade(de,ALPHA); 37 | CY_fbdr = eval_CY_fbdr(dr,BETA); 38 | Cl_fada = eval_Cl_fada(da,ALPHA); 39 | Cm_fa = eval_Cm_fa(ALPHA); 40 | Cm_fade = eval_Cm_fade(de,ALPHA); 41 | Cn_fbdr = eval_Cn_fbdr(dr,BETA); 42 | Cn_fada = eval_Cn_fada(da,ALPHA); 43 | end 44 | function[CD_fa] = eval_CD_fa(alpha); 45 | 46 | alpha1_table = linspace(-8,17,26); 47 | 48 | % CD_fa 49 | CD_falpha_lookup = [0.0734,0.0674,0.0629,0.0597, 0.0576,0.0568,0.0569,0.0580,0.0601,0.0630,0.0668,0.0713,0.0767,0.0830,0.0900,0.0980, 0.1069,0.1167,0.1276,0.1397,0.1530,0.2014,0.2209 0.1676,0.1837,0.2423]; 50 | 51 | 52 | CD_fa = interp1(alpha1_table,CD_falpha_lookup,alpha,'spline'); 53 | 54 | end 55 | function[CD_fade] = eval_CD_fade(de,alpha) 56 | 57 | % CD_fade 58 | alpha2_table = linspace (-7,17,25); 59 | 60 | alpha2_matrix = zeros(9,25); 61 | elev_def = linspace(-20,20,9); 62 | 63 | elev_matrix = zeros(9,25); 64 | 65 | for i = 1:9 66 | alpha2_matrix(i,:) = alpha2_table; 67 | elev_matrix(i,:) = elev_def(i); 68 | end; 69 | 70 | 71 | cd_ade_neg20 = [0.0231,0.0209,0.0187,0.0166,0.0147,0.0129,0.0113,0.0098,0.0086,0.0074,0.0063,0.0053,0.0043,0.0031,0.0017,0.0001,-0.002,-0.0046,-0.0079,-0.012,-0.0171,-0.0234,-0.0311,-0.0403,-0.0543]; 72 | 73 | cd_ade_neg15 = [0.0178,0.0155,0.0135,0.0117,0.0101,0.0086,0.0073,0.0061,0.005,0.004,0.003,0.002,0.001,0,-0.001,-0.002,-0.0031,-0.0042,-0.0054,-0.0067,-0.008,-0.0094,-0.0109,-0.0125,-0.0141]; 74 | 75 | cd_ade_neg10 = [0.0111,0.0095,0.0081,0.0069,0.0057,0.0047,0.0038,0.003,0.0022,0.0015,0.0008,0.0001,-0.0006,-0.0013,-0.002,-0.0027,-0.0035,-0.0042,-0.005,-0.0058,-0.0067,-0.0075,-0.0084,-0.0093,-0.0102]; 76 | 77 | cd_ade_neg5 = [0.0058,0.005,0.0043,0.0037,0.0031,0.0025,0.002,0.0016,0.0011,0.0007,0.0003,-0.0001,-0.0005,-0.0009,-0.0013,-0.0018,-0.0022,-0.0027,-0.0032,-0.0037,-0.0043,-0.0048,-0.0054,-0.006,-0.0066]; 78 | 79 | cd_ade_0 = linspace(0,0,25); 80 | 81 | cd_ade_5 = [-0.0028,-0.0021,-0.0015,-0.001,-0.0006,-0.0003,0,0.0003,0.0005,0.0007,0.001,0.0012,0.0014,0.0016,0.0018,0.002,0.0023,0.0025,0.0028,0.0031,0.0035,0.0038,0.0041,0.0045,0.0048]; 82 | 83 | cd_ade_10 = [-0.0053,-0.0037,-0.0024,-0.0014,-0.0005,0.0002,0.0008,0.0013,0.0018,0.0022,0.0026,0.003,0.0034,0.0039,0.0044,0.005,0.0056,0.0063,0.007,0.0078,0.0086,0.0095,0.0103,0.0111,0.0119]; 84 | 85 | cd_ade_15 = [-0.0063,-0.0043,-0.0025,-0.001,0.0002,0.0013,0.0022,0.0031,0.0039,0.0046,0.0053,0.006,0.0068,0.0076,0.0084,0.0093,0.0102,0.0112,0.0122,0.0133,0.0144,0.0155,0.0167,0.0177,0.0188]; 86 | 87 | cd_ade_20 = [-0.0029,-0.0007,0.0012,0.0028,0.0042,0.0053,0.0064,0.0074,0.0082,0.0091,0.0099,0.0108,0.0117,0.0126,0.0136,0.0147,0.0158,0.017,0.0182,0.0195,0.0209,0.0222,0.0236,0.025,0.0263]; 88 | 89 | CD_fade_lookup = [cd_ade_neg20;cd_ade_neg15;cd_ade_neg10;cd_ade_neg5;cd_ade_0;cd_ade_5;cd_ade_10;cd_ade_15;cd_ade_20]; 90 | 91 | CD_fade = griddata(elev_matrix,alpha2_matrix,CD_fade_lookup,de,alpha); 92 | 93 | end 94 | function[CL_fa] = eval_CL_fa(alpha); 95 | 96 | % Aerodynamic Lookup Tables 97 | alpha1_table = linspace(-8,17,26); 98 | 99 | %CL_fa 100 | CL_falpha_lookup = [-0.384,-0.294,-0.202,-0.107,-0.010,0.088,0.187,0.286,0.385,0.483,0.580,0.674,0.766,0.855,0.940,1.022,1.098,1.170,1.235,1.295,1.347,1.392,1.430,1.458,1.478,1.488]; 101 | 102 | 103 | CL_fa = interp1(alpha1_table,CL_falpha_lookup,alpha,'spline'); 104 | 105 | end 106 | function[Cl_fada] = eval_Cl_fada(da,alpha) 107 | 108 | % Cl_fada 109 | alpha3_table = [-10.31,-8.22,-6.12,-4.01,-1.91,0.2,2.29 ,4.4,6.5,8.59,10.65,12.71,13.72 ,14.73,15.74,16.75,17.75,18.75,19.74,20.73]; 110 | 111 | ail_def = linspace(-25,25,11); 112 | 113 | ail_matrix = zeros(11,20); 114 | alpha3_matrix = zeros(11,20); 115 | 116 | for i = 1:11 117 | ail_matrix(i,:) = ail_def(i); 118 | alpha3_matrix(i,:) = alpha3_table; 119 | end 120 | 121 | cl_ada_0 = linspace(0,0,20); 122 | cl_ada_5 = [-0.014,-0.0143,-0.0148,-0.0148,-0.0153,-0.0158,-0.016,-0.016,-0.0152,-0.0144,-0.0128,-0.0118,-0.0109,-0.0105,-0.0093,-0.0093,-0.0087,-0.0073,-0.0061,-0.0016]; 123 | cl_ada_10 = [-0.028,-0.0281,-0.0292,-0.0299,-0.0296,-0.0299,-0.0308,-0.0308,-0.0295,-0.0279,-0.026,-0.0238,-0.0222,-0.0202,-0.0177,-0.0167,-0.0147,-0.0105,-0.0066,-0.0028]; 124 | cl_ada_15 = [-0.0366,-0.0375,-0.0386,-0.0408,-0.04,-0.0405,-0.0424,-0.0424,-0.0422,-0.0407,-0.0382,-0.0364,-0.0332,-0.0307,-0.028,-0.0267,-0.0238,-0.0179,-0.013,-0.0077]; 125 | cl_ada_20 = [-0.0446,-0.0463,-0.0482,-0.0484,-0.0475,-0.0482,-0.0519,-0.0519,-0.0527,-0.0521,-0.0495,-0.0466,-0.0437,-0.0411,-0.0388,-0.036,-0.0333,-0.0285,-0.0226,-0.017]; 126 | cl_ada_25 = [-0.0533,-0.0553,-0.0568,-0.0563,-0.0568,-0.0573,-0.0604,-0.0604,-0.0614,-0.0616,-0.0597,-0.0558,-0.0524,-0.0487,-0.0461,-0.0423,-0.0376,-0.0342,-0.028,-0.0229]; 127 | cl_ada_neg5 = -1*cl_ada_5; 128 | cl_ada_neg10 = -1*cl_ada_10; 129 | cl_ada_neg15 = -1*cl_ada_15; 130 | cl_ada_neg20 = -1*cl_ada_20; 131 | cl_ada_neg25 = -1*cl_ada_25; 132 | 133 | Cl_fada_lookup = [cl_ada_neg25;cl_ada_neg20;cl_ada_neg15;cl_ada_neg10;cl_ada_neg5;cl_ada_0;cl_ada_5;cl_ada_10;cl_ada_15;cl_ada_20;cl_ada_25]; 134 | 135 | Cl_fada = griddata(ail_matrix,alpha3_matrix,Cl_fada_lookup,da,alpha); 136 | end 137 | function[CL_fade] = eval_CL_fade(de,alpha) 138 | 139 | % CL_fade 140 | alpha2_table = linspace (-7,17,25); 141 | elev_def = linspace(-20,20,9); 142 | elev_matrix = zeros(9,25); 143 | alpha2_matrix = zeros(9,25); 144 | 145 | for i=1:9 146 | elev_matrix(i,:) = elev_def(i); 147 | alpha2_matrix(i,:) = alpha2_table; 148 | end; 149 | 150 | cl_ade_neg20 = [-0.088,-0.093,-0.098,-0.103,-0.106,-0.11,-0.113,-0.116,-0.118,-0.119,-0.121,-0.122,-0.122,-0.123,-0.123,-0.122,-0.122,-0.121,-0.119,-0.118,-0.116,-0.114,-0.112,-0.109,-0.107]; 151 | 152 | cl_ade_neg15 = [-0.075,-0.079,-0.083,-0.087,-0.091,-0.094,-0.097,-0.1,-0.102,-0.105,-0.107,-0.108,-0.11,-0.111,-0.111,-0.111,-0.111,-0.11,-0.109,-0.108,-0.106,-0.103,-0.1,-0.097,-0.093]; 153 | 154 | cl_ade_neg10 = [-0.057,-0.061,-0.064,-0.067,-0.07,-0.072,-0.074,-0.076,-0.078,-0.079,-0.079,-0.08,-0.08,-0.08,-0.08,-0.079,-0.078,-0.077,-0.075,-0.073,-0.071,-0.068,-0.065,-0.062,-0.058]; 155 | 156 | cl_ade_neg5 = [-0.036,-0.038,-0.04,-0.041,-0.043,-0.044,-0.045,-0.045,-0.045,-0.045,-0.045,-0.045,-0.044,-0.043,-0.042,-0.041,-0.04,-0.038,-0.037,-0.035,-0.033,-0.031,-0.029,-0.027,-0.025]; 157 | 158 | cl_ade_0 = linspace(0,0,25); 159 | 160 | cl_ade_5 = [0.025,0.026,0.027,0.027,0.028,0.029,0.03,0.03,0.031,0.031,0.032,0.032,0.032,0.032,0.032,0.032,0.032,0.032,0.031,0.03,0.029,0.028,0.027,0.025,0.024]; 161 | 162 | cl_ade_10 =[0.058,0.059,0.06,0.061,0.062,0.063,0.064,0.065,0.066,0.066,0.067,0.067,0.068,0.068,0.068,0.068,0.067,0.066,0.065,0.064,0.062,0.061,0.058,0.055,0.052]; 163 | 164 | cl_ade_15 = [0.093,0.093,0.094,0.095,0.096,0.097,0.098,0.099,0.099,0.1,0.1,0.1,0.1,0.1,0.099,0.098,0.096,0.094,0.092,0.089,0.086,0.082,0.078,0.073,0.068]; 165 | 166 | cl_ade_20 = [0.1,0.101,0.101,0.102,0.103,0.104,0.1060,0.107,0.108,0.11,0.111,0.112,0.113,0.113,0.114,0.113,0.113,0.111,0.11,0.107,0.104,0.1,0.096,0.09,0.084]; 167 | 168 | CL_fade_lookup = [cl_ade_neg20;cl_ade_neg15;cl_ade_neg10;cl_ade_neg5;cl_ade_0;cl_ade_5;cl_ade_10;cl_ade_15;cl_ade_20]; 169 | 170 | CL_fade = griddata(elev_matrix,alpha2_matrix,CL_fade_lookup,de,alpha); 171 | 172 | end 173 | function[Cm_fa] = eval_Cm_fa(alpha) 174 | 175 | alpha = 1; 176 | % Aerodynamic Lookup Tables 177 | alpha1_table = linspace(-8,17,26); 178 | 179 | % Cm_fa 180 | Cm_falpha_lookup = [0.411,0.39,0.365,0.338,0.309,0.279,0.251,0.222,0.194,0.167,0.139,0.11,0.08,0.047,0.012,-0.027,-0.07,-0.116, -0.168,-0.223,-0.281,-0.342,-0.404,-0.465,-0.522,-0.573]; 181 | 182 | Cm_fa = interp1(alpha1_table,Cm_falpha_lookup,alpha); 183 | 184 | end 185 | function[Cm_fade] = eval_Cm_fade(de,alpha) 186 | 187 | 188 | % Cm_fade 189 | alpha4_table = [-6,-4,-2,0,2,4,6,8,10,12,13,14,15,16,17,18,19,20]; 190 | elev2_def = linspace(-25,20,10); 191 | 192 | elev2_matrix = zeros(10,18); 193 | alpha4_matrix = zeros(10,18); 194 | 195 | for i= 1:10 196 | elev2_matrix(i,:) = elev2_def(i); 197 | alpha4_matrix(i,:) = alpha4_table; 198 | end; 199 | 200 | cm_ade_neg25 = [0.9219,0.8735,0.827,0.7798,0.7413,0.6706,0.587,0.5293,0.4276,0.3045,0.2379,0.1698,0.1088,0.0503,-0.006,-0.0704,-0.1576,-0.2624]; 201 | 202 | cm_ade_neg20 = [0.8532,0.8031,0.7546,0.7073,0.6698,0.5983,0.5275,0.4822,0.3743,0.252,0.1943,0.1367,0.0721,0.0109,-0.0452,-0.1072,-0.1668,-0.2034]; 203 | 204 | cm_ade_neg15 = [0.7722,0.7138,0.6717,0.6258,0.5939,0.5384,0.4621,0.4109,0.3178,0.2058,0.1426,0.0705,0.0028,-0.0594,-0.123,-0.1827,-0.2384,-0.2822]; 205 | 206 | cm_ade_neg10 = [0.6464,0.5998,0.559,0.5156,0.4682,0.4039,0.3265,0.2551,0.1513,0.0404,-0.0216,-0.0911,-0.1512,-0.2198,-0.2834,-0.3425,-0.3938,-0.4234]; 207 | 208 | cm_ade_neg5 = [0.5099,0.4673,0.4248,0.3705,0.3111,0.2426,0.1675,0.0901,-0.0115,-0.1199,-0.1775,-0.2419,-0.3048,-0.3693,-0.429,-0.4869,-0.5246,-0.5369]; 209 | 210 | cm_ade_0 = [0.3524,0.305,0.2505,0.1922,0.1331,0.0681,-0.0041,-0.0826,-0.1771,-0.285,-0.3435,-0.4058,-0.4655,-0.5284,-0.583,-0.626,-0.6414,-0.6366]; 211 | 212 | cm_ade_5 = [0.2042,0.1561,0.1051,0.0403,-0.0198,-0.0833,-0.1556,-0.2472,-0.3376,-0.4361,-0.4882,-0.5455,-0.6097,-0.6657,-0.7117,-0.7409,-0.7367,-0.726]; 213 | 214 | cm_ade_10 = [0.0376,-0.0146,-0.075,-0.1391,-0.1921,-0.2594,-0.3327,-0.4179,-0.5038,-0.5937,-0.6446,-0.7024,-0.7536,-0.8052,-0.8406,-0.8481,-0.832,-0.8175]; 215 | 216 | cm_ade_15 = [-0.11,-0.1606,-0.2137,-0.2678,-0.3162,-0.378,-0.4518,-0.5384,-0.6162,-0.6994,-0.7465,-0.8015,-0.8507,-0.903,-0.9229,-0.9051,-0.8863,-0.8832]; 217 | 218 | cm_ade_20 = [-0.1353,-0.1817,-0.2356,-0.3001,-0.3675,-0.4532,-0.5296,-0.6121,-0.6925,-0.7772,-0.8266,-0.8897,-0.9399,-0.9818,-0.9877,-0.9575,-0.9439,-0.9447]; 219 | 220 | Cm_fade_lookup = [cm_ade_neg25;cm_ade_neg20;cm_ade_neg15;cm_ade_neg10;cm_ade_neg5;cm_ade_0;cm_ade_5;cm_ade_10;cm_ade_15;cm_ade_20]; 221 | 222 | Cm_fade = griddata(elev2_matrix,alpha4_matrix,Cm_fade_lookup,de,alpha); 223 | end 224 | function[Cn_fada] = eval_Cn_fada(da,alpha); 225 | 226 | % Cn_ada 227 | alpha1_table = linspace(-8,17,26); 228 | 229 | ail_def = linspace(-25,25,11); 230 | 231 | ail_matrix = zeros(11,26); 232 | alpha1_matrix = zeros(11,26); 233 | 234 | for i = 1:11 235 | ail_matrix(i,:) = ail_def(i); 236 | alpha1_matrix(i,:) = alpha1_table; 237 | end; 238 | 239 | 240 | 241 | 242 | cn_ada_0 = linspace(0,0,26); 243 | 244 | cn_ada_5 = [0.0004,0.0002,0.0001,-0.0001,-0.0002,-0.0004,-0.0005,-0.0007,-0.0008,-0.001,-0.0011,-0.0013,-0.0015,-0.0016,-0.0018,-0.0019,-0.0021,-0.0022,-0.0024,-0.0026,-0.0027,-0.0029,-0.003,-0.0032,-0.0034,-0.0035]; 245 | 246 | cn_ada_10 = [0.0013,0.0009,0.0004,0,-0.0004,-0.0007,-0.0011,-0.0015,-0.0018,-0.0022,-0.0025,-0.0029,-0.0032,-0.0035,-0.0038,-0.0041,-0.0043,-0.0046,-0.0049,-0.0051,-0.0054,-0.0056,-0.0058,-0.006,-0.0062,-0.0064]; 247 | 248 | cn_ada_15 = [0.0014,0.0008,0.0003,-0.0002,-0.0007,-0.0011,-0.0016,-0.0021,-0.0026,-0.003,-0.0035,-0.004,-0.0044,-0.0049,-0.0053,-0.0057,-0.0062,-0.0066,-0.007,-0.0074,-0.0078,-0.0082,-0.0086,-0.009,-0.0094,-0.0098]; 249 | 250 | cn_ada_20 = [0.0022,0.0015,0.0008,0.0001,-0.0005,-0.0012,-0.0019,-0.0025,-0.0032,-0.0038,-0.0044,-0.005,-0.0057,-0.0063,-0.0069,-0.0075,-0.0081,-0.0087,-0.0093,-0.0098,-0.0104,-0.011,-0.0115,-0.0121,-0.0126,-0.0132]; 251 | 252 | cn_ada_25 = [0.0025,0.0018,0.001,0.0003,-0.0004,-0.0012,-0.0019,-0.0026,0.0033,-0.004,-0.0047,-0.0054,-0.0061,-0.0068,-0.0074,-0.0081,-0.0088,-0.0094,-0.0101,-0.0107,-0.0114,-0.012,-0.0127,-0.0133,-0.0139,-0.0145]; 253 | 254 | cn_ada_neg5 = -1*cn_ada_5; 255 | cn_ada_neg10 = -1*cn_ada_10; 256 | cn_ada_neg15 = -1*cn_ada_15; 257 | cn_ada_neg20 = -1*cn_ada_20; 258 | cn_ada_neg25 = -1*cn_ada_25; 259 | 260 | Cn_fada_lookup = [cn_ada_neg25;cn_ada_neg20;cn_ada_neg15;cn_ada_neg10;cn_ada_neg5;cn_ada_0;cn_ada_5;cn_ada_10;cn_ada_15;cn_ada_20;cn_ada_25]; 261 | 262 | 263 | Cn_fada = griddata(ail_matrix,alpha1_matrix,Cn_fada_lookup,da,alpha); 264 | end 265 | function[Cn_fbdr] = eval_Cn_fbdr(dr,Beta) 266 | 267 | %Cn_fbdr 268 | Beta2_table = [-20,-17.5,-15,-12.5,-10,-7.5,-5,-2.5,0,2.5,5,7.5,10,11.25,12.5,13.75,15,16.25,17.5,18.75,20]; 269 | 270 | rdr2_def = linspace(-25,25,11); 271 | 272 | Beta2_matrix = zeros(11,21); 273 | rdr2_matrix = zeros(11,21); 274 | 275 | for i=1:11 276 | 277 | rdr2_matrix(i,:) = rdr2_def(i); 278 | Beta2_matrix(i,:) = Beta2_table; 279 | end; 280 | 281 | cn_bdr_neg25 = [0.0067,0,0.004,0.0087,0.0121,0.0161,0.02,0.0275,0.0335,0.0382,0.0429,0.0476,0.0516,0.0549,0.0563,0.057,0.0523,0.0503,0.0509,0.0509,0.0509]; 282 | 283 | cn_bdr_neg20 = [0.0027,0.0013,0.002,0.0074,0.0101,0.0127,0.0174,0.0235,0.0295,0.0348,0.0389,0.0429,0.0462,0.0489,0.0509,0.0509,0.0482,0.0442,0.0428,0.0436,0.0442]; 284 | 285 | cn_bdr_neg15 = [-0.0027,-0.0054,-0.004,0.0027,0.0054,0.0087,0.0134,0.0188,0.0255,0.0288,0.0328,0.0375,0.0422,0.0442,0.0462,0.0469,0.0429,0.0415,0.0422,0.0429,0.0429]; 286 | 287 | cn_bdr_neg10 = [-0.0094,-0.0127,-0.0114,-0.0047,-0.0014,0.0013,0.0067,0.0127,0.0188,0.0228,0.0261,0.0288,0.0348,0.0375,0.0395,0.0402,0.0369,0.0355,0.0355,0.0362,0.0369]; 288 | 289 | cn_bdr_neg5 = [-0.0154,-0.0214,-0.02,-0.0134,-0.0101,-0.0064,-0.002,0.0047,0.0114,0.0154,0.0121,0.0248,0.0295,0.0315,0.0335,0.0342,0.0322,0.0308,0.0308,0.0308,0.0322]; 290 | 291 | cn_bdr_0 = [-0.0214,-0.0277,-0.0283,-0.0222,-0.0188,-0.0161,-0.0131,-0.0067,0,0.006,0.0101,0.0154,0.0188,0.0201,0.0235,0.0268,0.0255,0.0255,0.0235,0.0235,0.0248]; 292 | 293 | cn_bdr_5 = [-0.0274,-0.034,-0.0366,-0.031,-0.0275,-0.0258,-0.0242,-0.0181,-0.0074,-0.0034,0.0081,0.006,0.0081,0.0087,0.0135,0.0194,0.0188,0.0202,0.0162,0.0162,0.0174]; 294 | 295 | cn_bdr_10 = [-0.0334,-0.0427,-0.0452,-0.0397,-0.0362,-0.0335,-0.0329,-0.0261,-0.0148,-0.0108,-0.0059,0.002,0.0028,0.0027,0.0075,0.0134,0.0141,0.0155,0.0115,0.0108,0.0127]; 296 | 297 | cn_bdr_15 = [-0.0401,-0.05,-0.0526,-0.0471,-0.043,-0.0409,-0.0396,-0.0322,-0.0215,-0.0168,-0.0126,-0.0067,-0.0046,-0.004,0.0008,0.0067,0.0081,0.0095,0.0048,0.0041,0.0067]; 298 | 299 | cn_bdr_20 = [-0.0455,-0.0567,-0.586,-0.0052,-0.0477,-0.0449,-0.0436,-0.0369,-0.0255,-0.0228,-0.0187,-0.0121,-0.0086,-0.0088,-0.0039,0.0027,0.0001,0.0068,0.0042,0.0034,0.0054]; 300 | 301 | cn_bdr_25 = [-0.0495,-0.0554,-0.0606,-0.0531,-0.0497,-0.0483,-0.0462,-0.0409,-0.0295,-0.0262,-0.0227,-0.0168,-0.014,-0.0147,-0.0093,-0.0034,-0.0013,0.0007,-0.0039,-0.0039,-0.0013]; 302 | 303 | Cn_fbdr_lookup = [cn_bdr_neg25;cn_bdr_neg20;cn_bdr_neg15;cn_bdr_neg10;cn_bdr_neg5;cn_bdr_0;cn_bdr_5;cn_bdr_10;cn_bdr_15;cn_bdr_20;cn_bdr_25]; 304 | 305 | Cn_fbdr = griddata(rdr2_matrix,Beta2_matrix,Cn_fbdr_lookup,dr,Beta); 306 | 307 | end 308 | function[CY_fbdr] = eval_CY_fbdr(dr,Beta); 309 | 310 | % CY_fbdr 311 | Beta_table = [-20,0,20]; 312 | Beta_matrix = zeros(3,3); 313 | 314 | rdr_def = [-20,0,20]; 315 | rdr_matrix = zeros(3,3); 316 | 317 | for i = 1:3 318 | Beta_matrix(i,:) = Beta_table; 319 | rdr_matrix(i,:) = rdr_def(i); 320 | end 321 | 322 | 323 | cy_bdr_neg20 = [0.19,0.24,0.29]; 324 | cy_bdr_0 = [-0.05,0,0.05]; 325 | cy_bdr_20 = [-0.25,-0.22,-0.18]; 326 | 327 | CY_fbdr_lookup = [cy_bdr_neg20;cy_bdr_0;cy_bdr_20]; 328 | 329 | 330 | CY_fbdr = griddata(rdr_matrix,Beta_matrix,CY_fbdr_lookup, dr, Beta); 331 | 332 | end -------------------------------------------------------------------------------- /eval_aerod_coeff.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Aerodynamic Coefficient Calculation 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | function [CD_fa,CD_fade,CL_fa,CL_fade,CY_fbdr,Cl_fada,Cm_fa,Cm_fade,Cn_fbdr,Cn_fada] = eval_aerod_coeff(de,da,dr,ALPHA,BETA); 9 | CD_fa = eval_CD_fa(ALPHA); 10 | CD_fade = eval_CD_fade(de,ALPHA); 11 | CL_fa = eval_CL_fa(ALPHA); 12 | CL_fade = eval_CL_fade(de,ALPHA); 13 | CY_fbdr = eval_CY_fbdr(dr,BETA); 14 | Cl_fada = eval_Cl_fada(da,ALPHA); 15 | Cm_fa = eval_Cm_fa(ALPHA); 16 | Cm_fade = eval_Cm_fade(de,ALPHA); 17 | Cn_fbdr = eval_Cn_fbdr(dr,BETA); 18 | Cn_fada = eval_Cn_fada(da,ALPHA); 19 | end 20 | function[CD_fa] = eval_CD_fa(alpha); 21 | 22 | alpha1_table = linspace(-8,17,26); 23 | 24 | % CD_fa 25 | CD_falpha_lookup = [0.0734,0.0674,0.0629,0.0597, 0.0576,0.0568,0.0569,0.0580,0.0601,0.0630,0.0668,0.0713,0.0767,0.0830,0.0900,0.0980, 0.1069,0.1167,0.1276,0.1397,0.1530,0.2014,0.2209 0.1676,0.1837,0.2423]; 26 | 27 | 28 | CD_fa = interp1(alpha1_table,CD_falpha_lookup,alpha,'spline'); 29 | 30 | end 31 | function[CD_fade] = eval_CD_fade(de,alpha) 32 | 33 | % CD_fade 34 | alpha2_table = linspace (-7,17,25); 35 | 36 | alpha2_matrix = zeros(9,25); 37 | elev_def = linspace(-20,20,9); 38 | 39 | elev_matrix = zeros(9,25); 40 | 41 | for i = 1:9 42 | alpha2_matrix(i,:) = alpha2_table; 43 | elev_matrix(i,:) = elev_def(i); 44 | end; 45 | 46 | 47 | cd_ade_neg20 = [0.0231,0.0209,0.0187,0.0166,0.0147,0.0129,0.0113,0.0098,0.0086,0.0074,0.0063,0.0053,0.0043,0.0031,0.0017,0.0001,-0.002,-0.0046,-0.0079,-0.012,-0.0171,-0.0234,-0.0311,-0.0403,-0.0543]; 48 | 49 | cd_ade_neg15 = [0.0178,0.0155,0.0135,0.0117,0.0101,0.0086,0.0073,0.0061,0.005,0.004,0.003,0.002,0.001,0,-0.001,-0.002,-0.0031,-0.0042,-0.0054,-0.0067,-0.008,-0.0094,-0.0109,-0.0125,-0.0141]; 50 | 51 | cd_ade_neg10 = [0.0111,0.0095,0.0081,0.0069,0.0057,0.0047,0.0038,0.003,0.0022,0.0015,0.0008,0.0001,-0.0006,-0.0013,-0.002,-0.0027,-0.0035,-0.0042,-0.005,-0.0058,-0.0067,-0.0075,-0.0084,-0.0093,-0.0102]; 52 | 53 | cd_ade_neg5 = [0.0058,0.005,0.0043,0.0037,0.0031,0.0025,0.002,0.0016,0.0011,0.0007,0.0003,-0.0001,-0.0005,-0.0009,-0.0013,-0.0018,-0.0022,-0.0027,-0.0032,-0.0037,-0.0043,-0.0048,-0.0054,-0.006,-0.0066]; 54 | 55 | cd_ade_0 = linspace(0,0,25); 56 | 57 | cd_ade_5 = [-0.0028,-0.0021,-0.0015,-0.001,-0.0006,-0.0003,0,0.0003,0.0005,0.0007,0.001,0.0012,0.0014,0.0016,0.0018,0.002,0.0023,0.0025,0.0028,0.0031,0.0035,0.0038,0.0041,0.0045,0.0048]; 58 | 59 | cd_ade_10 = [-0.0053,-0.0037,-0.0024,-0.0014,-0.0005,0.0002,0.0008,0.0013,0.0018,0.0022,0.0026,0.003,0.0034,0.0039,0.0044,0.005,0.0056,0.0063,0.007,0.0078,0.0086,0.0095,0.0103,0.0111,0.0119]; 60 | 61 | cd_ade_15 = [-0.0063,-0.0043,-0.0025,-0.001,0.0002,0.0013,0.0022,0.0031,0.0039,0.0046,0.0053,0.006,0.0068,0.0076,0.0084,0.0093,0.0102,0.0112,0.0122,0.0133,0.0144,0.0155,0.0167,0.0177,0.0188]; 62 | 63 | cd_ade_20 = [-0.0029,-0.0007,0.0012,0.0028,0.0042,0.0053,0.0064,0.0074,0.0082,0.0091,0.0099,0.0108,0.0117,0.0126,0.0136,0.0147,0.0158,0.017,0.0182,0.0195,0.0209,0.0222,0.0236,0.025,0.0263]; 64 | 65 | CD_fade_lookup = [cd_ade_neg20;cd_ade_neg15;cd_ade_neg10;cd_ade_neg5;cd_ade_0;cd_ade_5;cd_ade_10;cd_ade_15;cd_ade_20]; 66 | 67 | CD_fade = griddata(elev_matrix,alpha2_matrix,CD_fade_lookup,de,alpha); 68 | 69 | end 70 | function[CL_fa] = eval_CL_fa(alpha); 71 | 72 | % Aerodynamic Lookup Tables 73 | alpha1_table = linspace(-8,17,26); 74 | 75 | %CL_fa 76 | CL_falpha_lookup = [-0.384,-0.294,-0.202,-0.107,-0.010,0.088,0.187,0.286,0.385,0.483,0.580,0.674,0.766,0.855,0.940,1.022,1.098,1.170,1.235,1.295,1.347,1.392,1.430,1.458,1.478,1.488]; 77 | 78 | 79 | CL_fa = interp1(alpha1_table,CL_falpha_lookup,alpha,'spline'); 80 | 81 | end 82 | function[Cl_fada] = eval_Cl_fada(da,alpha) 83 | 84 | % Cl_fada 85 | alpha3_table = [-10.31,-8.22,-6.12,-4.01,-1.91,0.2,2.29 ,4.4,6.5,8.59,10.65,12.71,13.72 ,14.73,15.74,16.75,17.75,18.75,19.74,20.73]; 86 | 87 | ail_def = linspace(-25,25,11); 88 | 89 | ail_matrix = zeros(11,20); 90 | alpha3_matrix = zeros(11,20); 91 | 92 | for i = 1:11 93 | ail_matrix(i,:) = ail_def(i); 94 | alpha3_matrix(i,:) = alpha3_table; 95 | end 96 | 97 | cl_ada_0 = linspace(0,0,20); 98 | cl_ada_5 = [-0.014,-0.0143,-0.0148,-0.0148,-0.0153,-0.0158,-0.016,-0.016,-0.0152,-0.0144,-0.0128,-0.0118,-0.0109,-0.0105,-0.0093,-0.0093,-0.0087,-0.0073,-0.0061,-0.0016]; 99 | cl_ada_10 = [-0.028,-0.0281,-0.0292,-0.0299,-0.0296,-0.0299,-0.0308,-0.0308,-0.0295,-0.0279,-0.026,-0.0238,-0.0222,-0.0202,-0.0177,-0.0167,-0.0147,-0.0105,-0.0066,-0.0028]; 100 | cl_ada_15 = [-0.0366,-0.0375,-0.0386,-0.0408,-0.04,-0.0405,-0.0424,-0.0424,-0.0422,-0.0407,-0.0382,-0.0364,-0.0332,-0.0307,-0.028,-0.0267,-0.0238,-0.0179,-0.013,-0.0077]; 101 | cl_ada_20 = [-0.0446,-0.0463,-0.0482,-0.0484,-0.0475,-0.0482,-0.0519,-0.0519,-0.0527,-0.0521,-0.0495,-0.0466,-0.0437,-0.0411,-0.0388,-0.036,-0.0333,-0.0285,-0.0226,-0.017]; 102 | cl_ada_25 = [-0.0533,-0.0553,-0.0568,-0.0563,-0.0568,-0.0573,-0.0604,-0.0604,-0.0614,-0.0616,-0.0597,-0.0558,-0.0524,-0.0487,-0.0461,-0.0423,-0.0376,-0.0342,-0.028,-0.0229]; 103 | cl_ada_neg5 = -1*cl_ada_5; 104 | cl_ada_neg10 = -1*cl_ada_10; 105 | cl_ada_neg15 = -1*cl_ada_15; 106 | cl_ada_neg20 = -1*cl_ada_20; 107 | cl_ada_neg25 = -1*cl_ada_25; 108 | 109 | Cl_fada_lookup = [cl_ada_neg25;cl_ada_neg20;cl_ada_neg15;cl_ada_neg10;cl_ada_neg5;cl_ada_0;cl_ada_5;cl_ada_10;cl_ada_15;cl_ada_20;cl_ada_25]; 110 | 111 | Cl_fada = griddata(ail_matrix,alpha3_matrix,Cl_fada_lookup,da,alpha); 112 | end 113 | function[CL_fade] = eval_CL_fade(de,alpha) 114 | 115 | % CL_fade 116 | alpha2_table = linspace (-7,17,25); 117 | elev_def = linspace(-20,20,9); 118 | elev_matrix = zeros(9,25); 119 | alpha2_matrix = zeros(9,25); 120 | 121 | for i=1:9 122 | elev_matrix(i,:) = elev_def(i); 123 | alpha2_matrix(i,:) = alpha2_table; 124 | end; 125 | 126 | cl_ade_neg20 = [-0.088,-0.093,-0.098,-0.103,-0.106,-0.11,-0.113,-0.116,-0.118,-0.119,-0.121,-0.122,-0.122,-0.123,-0.123,-0.122,-0.122,-0.121,-0.119,-0.118,-0.116,-0.114,-0.112,-0.109,-0.107]; 127 | 128 | cl_ade_neg15 = [-0.075,-0.079,-0.083,-0.087,-0.091,-0.094,-0.097,-0.1,-0.102,-0.105,-0.107,-0.108,-0.11,-0.111,-0.111,-0.111,-0.111,-0.11,-0.109,-0.108,-0.106,-0.103,-0.1,-0.097,-0.093]; 129 | 130 | cl_ade_neg10 = [-0.057,-0.061,-0.064,-0.067,-0.07,-0.072,-0.074,-0.076,-0.078,-0.079,-0.079,-0.08,-0.08,-0.08,-0.08,-0.079,-0.078,-0.077,-0.075,-0.073,-0.071,-0.068,-0.065,-0.062,-0.058]; 131 | 132 | cl_ade_neg5 = [-0.036,-0.038,-0.04,-0.041,-0.043,-0.044,-0.045,-0.045,-0.045,-0.045,-0.045,-0.045,-0.044,-0.043,-0.042,-0.041,-0.04,-0.038,-0.037,-0.035,-0.033,-0.031,-0.029,-0.027,-0.025]; 133 | 134 | cl_ade_0 = linspace(0,0,25); 135 | 136 | cl_ade_5 = [0.025,0.026,0.027,0.027,0.028,0.029,0.03,0.03,0.031,0.031,0.032,0.032,0.032,0.032,0.032,0.032,0.032,0.032,0.031,0.03,0.029,0.028,0.027,0.025,0.024]; 137 | 138 | cl_ade_10 =[0.058,0.059,0.06,0.061,0.062,0.063,0.064,0.065,0.066,0.066,0.067,0.067,0.068,0.068,0.068,0.068,0.067,0.066,0.065,0.064,0.062,0.061,0.058,0.055,0.052]; 139 | 140 | cl_ade_15 = [0.093,0.093,0.094,0.095,0.096,0.097,0.098,0.099,0.099,0.1,0.1,0.1,0.1,0.1,0.099,0.098,0.096,0.094,0.092,0.089,0.086,0.082,0.078,0.073,0.068]; 141 | 142 | cl_ade_20 = [0.1,0.101,0.101,0.102,0.103,0.104,0.1060,0.107,0.108,0.11,0.111,0.112,0.113,0.113,0.114,0.113,0.113,0.111,0.11,0.107,0.104,0.1,0.096,0.09,0.084]; 143 | 144 | CL_fade_lookup = [cl_ade_neg20;cl_ade_neg15;cl_ade_neg10;cl_ade_neg5;cl_ade_0;cl_ade_5;cl_ade_10;cl_ade_15;cl_ade_20]; 145 | 146 | CL_fade = griddata(elev_matrix,alpha2_matrix,CL_fade_lookup,de,alpha); 147 | 148 | end 149 | function[Cm_fa] = eval_Cm_fa(alpha) 150 | 151 | alpha = 1; 152 | % Aerodynamic Lookup Tables 153 | alpha1_table = linspace(-8,17,26); 154 | 155 | % Cm_fa 156 | Cm_falpha_lookup = [0.411,0.39,0.365,0.338,0.309,0.279,0.251,0.222,0.194,0.167,0.139,0.11,0.08,0.047,0.012,-0.027,-0.07,-0.116, -0.168,-0.223,-0.281,-0.342,-0.404,-0.465,-0.522,-0.573]; 157 | 158 | Cm_fa = interp1(alpha1_table,Cm_falpha_lookup,alpha); 159 | 160 | end 161 | function[Cm_fade] = eval_Cm_fade(de,alpha) 162 | 163 | 164 | % Cm_fade 165 | alpha4_table = [-6,-4,-2,0,2,4,6,8,10,12,13,14,15,16,17,18,19,20]; 166 | elev2_def = linspace(-25,20,10); 167 | 168 | elev2_matrix = zeros(10,18); 169 | alpha4_matrix = zeros(10,18); 170 | 171 | for i= 1:10 172 | elev2_matrix(i,:) = elev2_def(i); 173 | alpha4_matrix(i,:) = alpha4_table; 174 | end; 175 | 176 | cm_ade_neg25 = [0.9219,0.8735,0.827,0.7798,0.7413,0.6706,0.587,0.5293,0.4276,0.3045,0.2379,0.1698,0.1088,0.0503,-0.006,-0.0704,-0.1576,-0.2624]; 177 | 178 | cm_ade_neg20 = [0.8532,0.8031,0.7546,0.7073,0.6698,0.5983,0.5275,0.4822,0.3743,0.252,0.1943,0.1367,0.0721,0.0109,-0.0452,-0.1072,-0.1668,-0.2034]; 179 | 180 | cm_ade_neg15 = [0.7722,0.7138,0.6717,0.6258,0.5939,0.5384,0.4621,0.4109,0.3178,0.2058,0.1426,0.0705,0.0028,-0.0594,-0.123,-0.1827,-0.2384,-0.2822]; 181 | 182 | cm_ade_neg10 = [0.6464,0.5998,0.559,0.5156,0.4682,0.4039,0.3265,0.2551,0.1513,0.0404,-0.0216,-0.0911,-0.1512,-0.2198,-0.2834,-0.3425,-0.3938,-0.4234]; 183 | 184 | cm_ade_neg5 = [0.5099,0.4673,0.4248,0.3705,0.3111,0.2426,0.1675,0.0901,-0.0115,-0.1199,-0.1775,-0.2419,-0.3048,-0.3693,-0.429,-0.4869,-0.5246,-0.5369]; 185 | 186 | cm_ade_0 = [0.3524,0.305,0.2505,0.1922,0.1331,0.0681,-0.0041,-0.0826,-0.1771,-0.285,-0.3435,-0.4058,-0.4655,-0.5284,-0.583,-0.626,-0.6414,-0.6366]; 187 | 188 | cm_ade_5 = [0.2042,0.1561,0.1051,0.0403,-0.0198,-0.0833,-0.1556,-0.2472,-0.3376,-0.4361,-0.4882,-0.5455,-0.6097,-0.6657,-0.7117,-0.7409,-0.7367,-0.726]; 189 | 190 | cm_ade_10 = [0.0376,-0.0146,-0.075,-0.1391,-0.1921,-0.2594,-0.3327,-0.4179,-0.5038,-0.5937,-0.6446,-0.7024,-0.7536,-0.8052,-0.8406,-0.8481,-0.832,-0.8175]; 191 | 192 | cm_ade_15 = [-0.11,-0.1606,-0.2137,-0.2678,-0.3162,-0.378,-0.4518,-0.5384,-0.6162,-0.6994,-0.7465,-0.8015,-0.8507,-0.903,-0.9229,-0.9051,-0.8863,-0.8832]; 193 | 194 | cm_ade_20 = [-0.1353,-0.1817,-0.2356,-0.3001,-0.3675,-0.4532,-0.5296,-0.6121,-0.6925,-0.7772,-0.8266,-0.8897,-0.9399,-0.9818,-0.9877,-0.9575,-0.9439,-0.9447]; 195 | 196 | Cm_fade_lookup = [cm_ade_neg25;cm_ade_neg20;cm_ade_neg15;cm_ade_neg10;cm_ade_neg5;cm_ade_0;cm_ade_5;cm_ade_10;cm_ade_15;cm_ade_20]; 197 | 198 | Cm_fade = griddata(elev2_matrix,alpha4_matrix,Cm_fade_lookup,de,alpha); 199 | end 200 | function[Cn_fada] = eval_Cn_fada(da,alpha); 201 | 202 | % Cn_ada 203 | alpha1_table = linspace(-8,17,26); 204 | 205 | ail_def = linspace(-25,25,11); 206 | 207 | ail_matrix = zeros(11,26); 208 | alpha1_matrix = zeros(11,26); 209 | 210 | for i = 1:11 211 | ail_matrix(i,:) = ail_def(i); 212 | alpha1_matrix(i,:) = alpha1_table; 213 | end; 214 | 215 | 216 | 217 | 218 | cn_ada_0 = linspace(0,0,26); 219 | 220 | cn_ada_5 = [0.0004,0.0002,0.0001,-0.0001,-0.0002,-0.0004,-0.0005,-0.0007,-0.0008,-0.001,-0.0011,-0.0013,-0.0015,-0.0016,-0.0018,-0.0019,-0.0021,-0.0022,-0.0024,-0.0026,-0.0027,-0.0029,-0.003,-0.0032,-0.0034,-0.0035]; 221 | 222 | cn_ada_10 = [0.0013,0.0009,0.0004,0,-0.0004,-0.0007,-0.0011,-0.0015,-0.0018,-0.0022,-0.0025,-0.0029,-0.0032,-0.0035,-0.0038,-0.0041,-0.0043,-0.0046,-0.0049,-0.0051,-0.0054,-0.0056,-0.0058,-0.006,-0.0062,-0.0064]; 223 | 224 | cn_ada_15 = [0.0014,0.0008,0.0003,-0.0002,-0.0007,-0.0011,-0.0016,-0.0021,-0.0026,-0.003,-0.0035,-0.004,-0.0044,-0.0049,-0.0053,-0.0057,-0.0062,-0.0066,-0.007,-0.0074,-0.0078,-0.0082,-0.0086,-0.009,-0.0094,-0.0098]; 225 | 226 | cn_ada_20 = [0.0022,0.0015,0.0008,0.0001,-0.0005,-0.0012,-0.0019,-0.0025,-0.0032,-0.0038,-0.0044,-0.005,-0.0057,-0.0063,-0.0069,-0.0075,-0.0081,-0.0087,-0.0093,-0.0098,-0.0104,-0.011,-0.0115,-0.0121,-0.0126,-0.0132]; 227 | 228 | cn_ada_25 = [0.0025,0.0018,0.001,0.0003,-0.0004,-0.0012,-0.0019,-0.0026,0.0033,-0.004,-0.0047,-0.0054,-0.0061,-0.0068,-0.0074,-0.0081,-0.0088,-0.0094,-0.0101,-0.0107,-0.0114,-0.012,-0.0127,-0.0133,-0.0139,-0.0145]; 229 | 230 | cn_ada_neg5 = -1*cn_ada_5; 231 | cn_ada_neg10 = -1*cn_ada_10; 232 | cn_ada_neg15 = -1*cn_ada_15; 233 | cn_ada_neg20 = -1*cn_ada_20; 234 | cn_ada_neg25 = -1*cn_ada_25; 235 | 236 | Cn_fada_lookup = [cn_ada_neg25;cn_ada_neg20;cn_ada_neg15;cn_ada_neg10;cn_ada_neg5;cn_ada_0;cn_ada_5;cn_ada_10;cn_ada_15;cn_ada_20;cn_ada_25]; 237 | 238 | 239 | Cn_fada = griddata(ail_matrix,alpha1_matrix,Cn_fada_lookup,da,alpha); 240 | end 241 | function[Cn_fbdr] = eval_Cn_fbdr(dr,Beta) 242 | 243 | %Cn_fbdr 244 | Beta2_table = [-20,-17.5,-15,-12.5,-10,-7.5,-5,-2.5,0,2.5,5,7.5,10,11.25,12.5,13.75,15,16.25,17.5,18.75,20]; 245 | 246 | rdr2_def = linspace(-25,25,11); 247 | 248 | Beta2_matrix = zeros(11,21); 249 | rdr2_matrix = zeros(11,21); 250 | 251 | for i=1:11 252 | 253 | rdr2_matrix(i,:) = rdr2_def(i); 254 | Beta2_matrix(i,:) = Beta2_table; 255 | end; 256 | 257 | cn_bdr_neg25 = [0.0067,0,0.004,0.0087,0.0121,0.0161,0.02,0.0275,0.0335,0.0382,0.0429,0.0476,0.0516,0.0549,0.0563,0.057,0.0523,0.0503,0.0509,0.0509,0.0509]; 258 | 259 | cn_bdr_neg20 = [0.0027,0.0013,0.002,0.0074,0.0101,0.0127,0.0174,0.0235,0.0295,0.0348,0.0389,0.0429,0.0462,0.0489,0.0509,0.0509,0.0482,0.0442,0.0428,0.0436,0.0442]; 260 | 261 | cn_bdr_neg15 = [-0.0027,-0.0054,-0.004,0.0027,0.0054,0.0087,0.0134,0.0188,0.0255,0.0288,0.0328,0.0375,0.0422,0.0442,0.0462,0.0469,0.0429,0.0415,0.0422,0.0429,0.0429]; 262 | 263 | cn_bdr_neg10 = [-0.0094,-0.0127,-0.0114,-0.0047,-0.0014,0.0013,0.0067,0.0127,0.0188,0.0228,0.0261,0.0288,0.0348,0.0375,0.0395,0.0402,0.0369,0.0355,0.0355,0.0362,0.0369]; 264 | 265 | cn_bdr_neg5 = [-0.0154,-0.0214,-0.02,-0.0134,-0.0101,-0.0064,-0.002,0.0047,0.0114,0.0154,0.0121,0.0248,0.0295,0.0315,0.0335,0.0342,0.0322,0.0308,0.0308,0.0308,0.0322]; 266 | 267 | cn_bdr_0 = [-0.0214,-0.0277,-0.0283,-0.0222,-0.0188,-0.0161,-0.0131,-0.0067,0,0.006,0.0101,0.0154,0.0188,0.0201,0.0235,0.0268,0.0255,0.0255,0.0235,0.0235,0.0248]; 268 | 269 | cn_bdr_5 = [-0.0274,-0.034,-0.0366,-0.031,-0.0275,-0.0258,-0.0242,-0.0181,-0.0074,-0.0034,0.0081,0.006,0.0081,0.0087,0.0135,0.0194,0.0188,0.0202,0.0162,0.0162,0.0174]; 270 | 271 | cn_bdr_10 = [-0.0334,-0.0427,-0.0452,-0.0397,-0.0362,-0.0335,-0.0329,-0.0261,-0.0148,-0.0108,-0.0059,0.002,0.0028,0.0027,0.0075,0.0134,0.0141,0.0155,0.0115,0.0108,0.0127]; 272 | 273 | cn_bdr_15 = [-0.0401,-0.05,-0.0526,-0.0471,-0.043,-0.0409,-0.0396,-0.0322,-0.0215,-0.0168,-0.0126,-0.0067,-0.0046,-0.004,0.0008,0.0067,0.0081,0.0095,0.0048,0.0041,0.0067]; 274 | 275 | cn_bdr_20 = [-0.0455,-0.0567,-0.586,-0.0052,-0.0477,-0.0449,-0.0436,-0.0369,-0.0255,-0.0228,-0.0187,-0.0121,-0.0086,-0.0088,-0.0039,0.0027,0.0001,0.0068,0.0042,0.0034,0.0054]; 276 | 277 | cn_bdr_25 = [-0.0495,-0.0554,-0.0606,-0.0531,-0.0497,-0.0483,-0.0462,-0.0409,-0.0295,-0.0262,-0.0227,-0.0168,-0.014,-0.0147,-0.0093,-0.0034,-0.0013,0.0007,-0.0039,-0.0039,-0.0013]; 278 | 279 | Cn_fbdr_lookup = [cn_bdr_neg25;cn_bdr_neg20;cn_bdr_neg15;cn_bdr_neg10;cn_bdr_neg5;cn_bdr_0;cn_bdr_5;cn_bdr_10;cn_bdr_15;cn_bdr_20;cn_bdr_25]; 280 | 281 | Cn_fbdr = griddata(rdr2_matrix,Beta2_matrix,Cn_fbdr_lookup,dr,Beta); 282 | 283 | end 284 | function[CY_fbdr] = eval_CY_fbdr(dr,Beta); 285 | 286 | % CY_fbdr 287 | Beta_table = [-20,0,20]; 288 | Beta_matrix = zeros(3,3); 289 | 290 | rdr_def = [-20,0,20]; 291 | rdr_matrix = zeros(3,3); 292 | 293 | for i = 1:3 294 | Beta_matrix(i,:) = Beta_table; 295 | rdr_matrix(i,:) = rdr_def(i); 296 | end 297 | 298 | 299 | cy_bdr_neg20 = [0.19,0.24,0.29]; 300 | cy_bdr_0 = [-0.05,0,0.05]; 301 | cy_bdr_20 = [-0.25,-0.22,-0.18]; 302 | 303 | CY_fbdr_lookup = [cy_bdr_neg20;cy_bdr_0;cy_bdr_20]; 304 | 305 | 306 | CY_fbdr = griddata(rdr_matrix,Beta_matrix,CY_fbdr_lookup, dr, Beta); 307 | 308 | end -------------------------------------------------------------------------------- /eval_delay_iono.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Ionospheric Delay Calculation 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | % Function for computing an Ionospheric range correction for the * 9 | % GPS L1 frequency from the parameters broadcasted in the GPS * 10 | % Navigation Message. * 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | % References: * 13 | % Klobuchar, J.A., (1996) "Ionosphercic Effects on GPS", in * 14 | % Parkinson, Spilker (ed), "Global Positioning System Theory and * 15 | % Applications, pp.513-514. * 16 | % ICD-GPS-200, Rev. C, (1997), pp. 125-128 * 17 | % NATO, (1991), "Technical Characteristics of the NAVSTAR GPS", * 18 | % pp. A-6-31 - A-6-33 * 19 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 20 | % Input : * 21 | % user_pos_geodetic 22 | % El : elevation in radians % 23 | % A : azimuth in radians % 24 | % GPS_Time : Time of Week (sec) * 25 | % Alfa(4) : The coefficients of a cubic equation * 26 | % representing the amplitude of the vertical * 27 | % dalay (4 coefficients - 8 bits each) * 28 | % Beta(4) : The coefficients of a cubic equation * 29 | % representing the period of the model * 30 | % (4 coefficients - 8 bits each) * 31 | % Output: * 32 | % Iono_delay : Ionospheric slant range correction in sec * 33 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 34 | 35 | function [Iono_delay]= eval_delay_iono(user_pos_geodetic,El,A,gps_time); 36 | 37 | % Alpha and Beta parameters for Iono model taken from 38 | % ftp://cddis.gsfc.nasa.gov/pub/gps/data/daily 39 | Alpha = [0.1118E-07 0.0000E+00 -0.5960E-07 0.0000E+00]; 40 | Beta = [0.9011E+05 0.0000E+00 -0.1966E+06 0.0000E+00]; 41 | 42 | 43 | lat_user = user_pos_geodetic.lat/3.14159; % Convert to semi-circles 44 | 45 | long_user = user_pos_geodetic.long/3.14159; % Convert to semi-circles 46 | 47 | El = El/3.14159; % Convert to semi-circles 48 | 49 | %Calculate the earth centered angle 50 | Psi = 0.0137/(El+0.11)-0.022; %in semi-circles 51 | 52 | %Compute the subionospheric latitude 53 | Phi_subiono = lat_user + Psi*cos(A);% semi-circles; 54 | 55 | if Phi_subiono > 0.416 56 | 57 | Phi_subiono = 0.416; 58 | end; 59 | if Phi_subiono < -0.416 60 | Phi_subiono = -0.416; 61 | end; 62 | 63 | %Compute the subionospheric longitude 64 | lambda_subiono = long_user + Psi*sin(A)/cos(Phi_subiono); 65 | 66 | %compute the geomagnetic latitude 67 | Phi_m = Phi_subiono + 0.064*cos(lambda_subiono - 1.617); 68 | 69 | % find the local time 70 | t = 4.32*10000*lambda_subiono + gps_time; 71 | 72 | if t > 86400 73 | t = t - 86400; 74 | end; 75 | if t<0 76 | t = t + 86400; 77 | end; 78 | 79 | %compute the slant delay factor 80 | F = 1.0 + 16.0*(0.53-El)^3; 81 | 82 | Beta_factor=0; 83 | Alpha_factor = 0; 84 | for n=1:4 85 | Beta_factor = Beta_factor + Beta(n)*Phi_m^(n-1); 86 | Alpha_factor = Alpha_factor + Alpha(n)*Phi_m^(n-1); 87 | end; 88 | 89 | if Beta_factor < 72000 90 | Beta_factor=72000; 91 | end; 92 | 93 | if Alpha_factor < 0 94 | Alpha_factor = 0; 95 | end 96 | % Evaluate the delay 97 | 98 | x = 2*3.14159*(t - 50400)/(Beta_factor); 99 | 100 | if abs(x) > 1.57 101 | 102 | Iono_delay = F*5e-9; 103 | end; 104 | if abs(x) <= 1.57 105 | 106 | Iono_delay = F*(5e-9+Alpha_factor*(1-(x^2)/2+(x^4)/24)); 107 | end; 108 | 109 | end 110 | 111 | -------------------------------------------------------------------------------- /eval_delay_tropo.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Tropospheric Delay Calculation 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% * 8 | % Reference: 9 | % "Global Positioning System, Mishra & Enge", pg 172 * 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 11 | %Input 12 | % T_amb:Degree Celsius =>At reciever antenna location 13 | % P_amb:kPa =>At reciever antenna location 14 | % P_vap:kPa =>Water vapore pressure at reciever antenna location 15 | % E => Elevation angle in radians from user to satellite 16 | %Output 17 | % slant tropo_delay in seconds 18 | 19 | function [tropo_delay] = eval_delay_tropo(El,alt); 20 | 21 | [T_o,P_o] = atmosphere(alt); % local conditions 22 | P_vap = 20; % local vapour pressure (milli bar) 23 | c = 2.99792458e8;% speed of light (m/s) 24 | hd = 43000; 25 | hw = 12000; 26 | 27 | T_d = 77.6e-6*(P_o/T_o)*(hd/5); 28 | 29 | T_w = 0.373*(P_vap/T_o^2)*(hw/5); 30 | 31 | m_d = 1/(sin(El) + 0.00143/(tan(El)+0.0445)); 32 | 33 | m_w = 1/(sin(El)+ 0.00035/(tan(El)+0.017)); 34 | 35 | %Troposhpheric Delay 36 | tropo_delay = (T_d*m_d + T_w*m_w)/c; % in seconds 37 | end 38 | 39 | function [temp, pres] = atmosphere(alt) 40 | 41 | % CONSTANT PARAMETERS 42 | g = 9.86; % [m/s^2] 43 | temp_lapserate = 0.005; % [K/m] 44 | temp_sealevel = 308; % Temperature at sealevel[K] 45 | pressure_sealevel = 101325; % Pressure at sea level [pa] 46 | temp_alt = temp_sealevel - (alt * temp_lapserate); 47 | pressure_alt = pressure_sealevel * ((temp_sealevel/temp_alt)^(-g/(temp_lapserate*287.0))); 48 | temp = temp_alt; 49 | pres = pressure_alt*10^-2; % pressure in milli bar 50 | 51 | end 52 | -------------------------------------------------------------------------------- /eval_el_az.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Calculate Satellite Azimuth and Elevation from User to Satellite 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | % Reference: 9 | % Montenbruck and Gill pg. 37 % 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 11 | function [elevation_sv, azimuth_sv, is_visible] = eval_el_az(user_pos_geodetic,user_pos_ecef,sat_pos_ecef); 12 | 13 | user_lat = user_pos_geodetic.lat; 14 | user_long = user_pos_geodetic.long; 15 | 16 | e_E = [-sin(user_long) cos(user_long) 0]; 17 | e_N = [-sin(user_lat)*cos(user_long) -sin(user_lat)*sin(user_long) cos(user_lat)]; 18 | e_Z = [cos(user_lat)*cos(user_long) cos(user_lat)*sin(user_long) sin(user_lat)]; 19 | 20 | E = [e_E;e_N;e_Z]; 21 | 22 | s = E * [(sat_pos_ecef.x-user_pos_ecef.x); (sat_pos_ecef.y - user_pos_ecef.y); (sat_pos_ecef.z - user_pos_ecef.z)]; 23 | 24 | azimuth_sv = atan2(s(1),s(2)); 25 | elevation_sv = atan2(s(3),sqrt(s(1)^2+s(2)^2)); 26 | 27 | % check if satellite is visible from the user 28 | 29 | is_visible= 0; % default assumed satellite not visible 30 | 31 | if (0.17453< elevation_sv && elevation_sv < (3.14159 - 0.17453)) 32 | is_visible = 1; % incase it is visible, value gets changed to 1 33 | end; 34 | 35 | 36 | end -------------------------------------------------------------------------------- /eval_gaccel_body.asv: -------------------------------------------------------------------------------- 1 | %Calculates the gravitational acceleration given the ECEF position 2 | 3 | function gaccel_body = eval_gaccel_ned(pos_body_ecef, phi,theta,psi) 4 | 5 | 6 | mu = 3.986004415e14; %m^3/s^2 7 | a = 6378137; %m; equatorial radius of earth 8 | % b = 6356752.3142; %m; polar radius of earth 9 | J2 = 0.00108263; 10 | 11 | pos_body_geo = struct('lat',X(10),'long',X(11),'alt',X(12)); 12 | pos_body_ecef = latlong_to_ecef(pos_body_ecef); 13 | 14 | r1 = pos_body_ecef.x; 15 | r2 = pos_body_ecef.y; 16 | r3 = pos_body_ecef.z; 17 | r = sqrt(r1^2+r2^2+r3^2); 18 | Lg = r3/r; %geocentric latitude 19 | 20 | C_b2n = [cos(theta)*cos(psi) -cos(phi)*sin(psi)+sin(phi)*sin(theta)*cos(psi) sin(phi)*sin(psi)+cos(phi)*sin(theta)*cos(psi);cos(theta)*sin(psi) cos(phi)*cos(psi)+sin(phi)*sin(theta)*sin(psi) -sin(phi)*cos(psi)+cos(phi)*sin(theta)*sin(psi);-sin(theta) sin(phi)*cos(theta) cos(phi)*cos(theta)]; 21 | 22 | pos_body_geo = ecef_to_latlong(pos_body_ecef); 23 | 24 | lat = pos_body_geo.lat; 25 | 26 | long = pos_body_geo.long; 27 | 28 | C_ECEF2NED = [-sin(lat)*cos(long) -sin(lat)*sin(long) cos(lat);-sin(long) cos(long) 0; -cos(lat)*cos(long) -cos(lat)*sin(long) -sin(lat)]; 29 | 30 | gaccel_ECEF = -mu/r^2*... 31 | [(1+1.5*J2*(a/r)^2*(1-5*(Lg)^2))*r1/r;... 32 | (1+1.5*J2*(a/r)^2*(1-5*(Lg)^2))*r2/r;... 33 | (1+1.5*J2*(a/r)^2*(3-5*(Lg)^2))*r3/r]; 34 | 35 | gaccel_y = C_ECEF2NED*gaccel_ECEF; 36 | 37 | 38 | end 39 | %Ref: Page 41, Aircraft Control and Simulation by Stevens & Lewis -------------------------------------------------------------------------------- /eval_gaccel_ned.m: -------------------------------------------------------------------------------- 1 | %Calculates the gravitational acceleration given the ECEF position 2 | 3 | function gaccel_ned = eval_gaccel_ned(pos_body_geo) 4 | 5 | mu = 3.986004415e14; %m^3/s^2 6 | a = 6378137; %m; equatorial radius of earth 7 | % b = 6356752.3142; %m; polar radius of earth 8 | J2 = 0.00108263; 9 | 10 | pos_body_ecef = latlong_to_ecef(pos_body_geo); 11 | 12 | r1 = pos_body_ecef.x; 13 | r2 = pos_body_ecef.y; 14 | r3 = pos_body_ecef.z; 15 | r = sqrt(r1^2+r2^2+r3^2); 16 | Lg = r3/r; %geocentric latitude 17 | 18 | % C_b2n = [cos(theta)*cos(psi) -cos(phi)*sin(psi)+sin(phi)*sin(theta)*cos(psi) sin(phi)*sin(psi)+cos(phi)*sin(theta)*cos(psi);cos(theta)*sin(psi) cos(phi)*cos(psi)+sin(phi)*sin(theta)*sin(psi) -sin(phi)*cos(psi)+cos(phi)*sin(theta)*sin(psi);-sin(theta) sin(phi)*cos(theta) cos(phi)*cos(theta)]; 19 | 20 | pos_body_geo = ecef_to_latlong(pos_body_ecef); 21 | 22 | lat = pos_body_geo.lat; 23 | 24 | long = pos_body_geo.long; 25 | 26 | C_ECEF2NED = [-sin(lat)*cos(long) -sin(lat)*sin(long) cos(lat);-sin(long) cos(long) 0; -cos(lat)*cos(long) -cos(lat)*sin(long) -sin(lat)]; 27 | 28 | gaccel_ECEF = -mu/r^2*... 29 | [(1+1.5*J2*(a/r)^2*(1-5*(Lg)^2))*r1/r;... 30 | (1+1.5*J2*(a/r)^2*(1-5*(Lg)^2))*r2/r;... 31 | (1+1.5*J2*(a/r)^2*(3-5*(Lg)^2))*r3/r]; 32 | 33 | gaccel_ned = C_ECEF2NED*gaccel_ECEF; 34 | 35 | 36 | end 37 | %Ref: Page 41, Aircraft Control and Simulation by Stevens & Lewis -------------------------------------------------------------------------------- /eval_integer_ambiguity.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % This function evaluates the integer ambiguities 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | 9 | function [IntegerAmbiguities,AA,clkbias_u,clkbias_r] = eval_integer_ambiguity(no_of_measurement_epochs,time_of_epochs,countme,gps_sat,visible_sats_id,ref_station_ecef,ref_station,initial_user_pos_estimate_storage,true_user_pos_geodetic_storage,true_user_pos_ecef_storage,ibeacon1_geo,ibeacon1_ecef,ibeacon2_geo,ibeacon2_ecef,initial_clk_bias_u,initial_clk_bias_r); 10 | 11 | for i=1:countme 12 | 13 | gps_time = time_of_epochs(i); 14 | 15 | [deltaPhi,Sk,SmoothPhase,clkbias_u,clkbias_r] = collect_phase_data(gps_sat,gps_time,visible_sats_id,ref_station_ecef,ref_station,true_user_pos_geodetic_storage(i),true_user_pos_ecef_storage(i), initial_user_pos_estimate_storage(i), ibeacon1_geo,ibeacon1_ecef,ibeacon2_geo,ibeacon2_ecef,initial_clk_bias_u,initial_clk_bias_r); 16 | 17 | AA(:,i) = SmoothPhase; % Each column contains the smoothed phase (1st row contains N1 and so on) 18 | 19 | end; 20 | [rows,columns] = size(AA) 21 | for j = 1:rows 22 | IntegerAmbiguities(j) = (1/0.19)*sum(AA(j,:))/columns ; 23 | end; 24 | 25 | end -------------------------------------------------------------------------------- /eval_ned_to_ecef.m: -------------------------------------------------------------------------------- 1 | function [pos_ned] = eval_ned_to_ecef(ref_station, ref_station_ecef, user_pos_ned) 2 | lat = ref_station.lat; 3 | long = ref_station.long; 4 | 5 | xu = user_pos_ned.x; 6 | yu = user_pos_ned.y; 7 | zu = user_pos_ned.z; 8 | 9 | xr = ref_station_ecef.x; 10 | yr = ref_station_ecef.y; 11 | zr = ref_station_ecef.z; 12 | 13 | C_ECEF2NED = [-sin(lat)*cos(long) -sin(lat)*sin(long) cos(lat);-sin(long) cos(long) 0; -cos(lat)*cos(long) -cos(lat)*sin(long) -sin(lat)]; 14 | 15 | r_ecef = C_ECEF2NED'*[xu;yu;zu]; 16 | 17 | pos_ned = struct('x', r_ecef(1)+xr,'y',r_ecef(2)+yr,'z',r_ecef(3)+zr); 18 | end 19 | -------------------------------------------------------------------------------- /eval_pr_measurement.asv: -------------------------------------------------------------------------------- 1 | %%% This function evaluates the pseudo range measured by user %%%% 2 | %%% Author: Saurav Agarwal %%%%% 3 | 4 | function [pr_measured_L1, pr_measured_L2, pr_measured_L5] = eval_pr_measurement(gps_sat,sv_id,gps_time,true_user_pos_ecef, randomfactor,true_sat_pos_ecef) 5 | 6 | c = 2.99792458e8; 7 | 8 | % [sv_x,sv_y,sv_z] = calc_sat_pos_ecef(gps_sat,gps_time,sv_id); % The true position of satellite based on ephemeris data and the gps time 9 | % 10 | % true_sat_pos_ecef = struct('x',sv_x,'y',sv_y,'z',sv_z); % put this position in a struct 11 | 12 | [sat_clk_drift,sat_clk_rel_error] = eval_sat_clock_offset(gps_sat,sv_id,gps_time); % satellite clock offset in seconds (clock drift + relativistic error) 13 | 14 | time_str = gps_time + sat_clk_drift + sat_clk_rel_error; % The time of tranmission from satellite as embedded in transmitted code 15 | 16 | D = compute_distance(true_sat_pos_ecef,true_user_pos_ecef); % distance from user to satllite 17 | 18 | user_pos_geodetic = ecef_to_latlong(true_user_pos_ecef); % user position in geodetic frame 19 | 20 | [elev,azim,is_visible] = eval_el_az(user_pos_geodetic,true_user_pos_ecef,true_sat_pos_ecef); % gives elevation and azim in radians 21 | 22 | slant_iono_delay_L1 = eval_delay_iono(user_pos_geodetic,elev,azim,gps_time); % calculate L1 ionospheric delay in seconds 23 | 24 | slant_iono_delay_L2 = 1.6469*slant_iono_delay_L1; %calculate L2 ionospheric delay in seconds (from paper" a systems approach to ionospheric delay compenstaion") 25 | 26 | slant_tropo_delay = eval_delay_tropo(elev,user_pos_geodetic.alt); % calculate tropospheric delay in seconds 27 | 28 | rcvr_noise = randn/2; % Receiver error due to measurement noise 29 | 30 | if rcvr_noise > 0.5 31 | rcvr_noise = 0.5; 32 | end; 33 | if rcvr_noise <-0.5 34 | rcvr_noise = -0.5; 35 | end; 36 | 37 | rcvr_clk_bias = (rem(gps_time,600)+ randomfactor); % clock bias..the clock resets itself every 600 seconds 38 | 39 | time_rcvr_L1 = gps_time + D/c + slant_tropo_delay + slant_iono_delay_L1 + rcvr_clk_bias; % Time of receiving as seen by user 40 | 41 | time_rcvr_L2 = gps_time + D/c + slant_tropo_delay + slant_iono_delay_L2 + rcvr_clk_bias; % Time of receiving as seen by user 42 | 43 | pr_measured_L1 = c*(time_rcvr_L1 - time_str)+ rcvr_noise ; % computing the pseduo range which is measured by the receiver 44 | 45 | pr_measured_L2 = c*(time_rcvr_L2 - time_str)+ rcvr_noise ; % computing the pseduo range which is measured by the receiver 46 | 47 | A = (pr_measured_L1 - (2.546*pr_measured_L1 - 1.546*pr_measured_L2))*1575.42^2; 48 | 49 | pr_measured_L5 = 2.546*pr_measured_L1 - 1.546*pr_measured_L2 + A/1176.45^2; 50 | 51 | 52 | end -------------------------------------------------------------------------------- /eval_pr_measurement.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % This function evaluates the pseudo range measured by user 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | 9 | function [pr_measured_L1, pr_measured_L2, pr_measured_L5] = eval_pr_measurement(gps_sat,sv_id,gps_time,true_user_pos_ecef, rcvr_clk_bias,true_sat_pos_ecef) 10 | 11 | c = 2.99792458e8; 12 | 13 | % [sv_x,sv_y,sv_z] = calc_sat_pos_ecef(gps_sat,gps_time,sv_id); % The true position of satellite based on ephemeris data and the gps time 14 | % 15 | % true_sat_pos_ecef = struct('x',sv_x,'y',sv_y,'z',sv_z); % put this position in a struct 16 | 17 | [sat_clk_drift,sat_clk_rel_error] = eval_sat_clock_offset(gps_sat,sv_id,gps_time); % satellite clock offset in seconds (clock drift + relativistic error) 18 | 19 | time_str = gps_time + sat_clk_drift + sat_clk_rel_error; % The time of tranmission from satellite as embedded in transmitted code 20 | 21 | D = compute_distance(true_sat_pos_ecef,true_user_pos_ecef); % distance from user to satllite 22 | 23 | user_pos_geodetic = ecef_to_latlong(true_user_pos_ecef); % user position in geodetic frame 24 | 25 | [elev,azim,is_visible] = eval_el_az(user_pos_geodetic,true_user_pos_ecef,true_sat_pos_ecef); % gives elevation and azim in radians 26 | 27 | slant_iono_delay_L1 = eval_delay_iono(user_pos_geodetic,elev,azim,gps_time); % calculate L1 ionospheric delay in seconds 28 | 29 | slant_iono_delay_L2 = 1.6469*slant_iono_delay_L1; %calculate L2 ionospheric delay in seconds (from paper" a systems approach to ionospheric delay compenstaion") 30 | 31 | slant_tropo_delay = eval_delay_tropo(elev,user_pos_geodetic.alt); % calculate tropospheric delay in seconds 32 | 33 | rcvr_noise = randn; % Receiver error due to measurement noise 34 | 35 | if rcvr_noise > 0.5 36 | rcvr_noise = 0.5; 37 | end; 38 | if rcvr_noise <-0.5 39 | rcvr_noise = -0.5; 40 | end; 41 | 42 | time_rcvr_L1 = gps_time + D/c + slant_tropo_delay + slant_iono_delay_L1 + rcvr_clk_bias; % Time of receiving as seen by user 43 | 44 | time_rcvr_L2 = gps_time + D/c + slant_tropo_delay + slant_iono_delay_L2 + rcvr_clk_bias; % Time of receiving as seen by user 45 | 46 | pr_measured_L1 = c*(time_rcvr_L1 - time_str)+ rcvr_noise ; % computing the pseduo range which is measured by the receiver 47 | 48 | pr_measured_L2 = c*(time_rcvr_L2 - time_str)+ rcvr_noise ; % computing the pseduo range which is measured by the receiver 49 | 50 | A = (pr_measured_L1 - (2.546*pr_measured_L1 - 1.546*pr_measured_L2))*1575.42^2; 51 | 52 | pr_measured_L5 = 2.546*pr_measured_L1 - 1.546*pr_measured_L2 + A/1176.45^2; 53 | 54 | 55 | end -------------------------------------------------------------------------------- /eval_sat_clock_offset.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Calculate Satellite Offest Clock Error 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | % Reference:"GPS Theory and application",edited by B.Parkinson,J.Spilker, 9 | %Ofset Model: 10 | %dTclk_Ofset=af0+af1*(T-Toc)+af2*(T-Toc)^2+..... 11 | %af :(1/Sec^i) =>Matrix of Coeeficient for satellite offset 12 | %Ttr:(Sec) => Time of transmission 13 | %Toc:(Sec) => Sv Clock refernce time 14 | %dTclk_Ofset:(Sec)=> Sv Clock offset time 15 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 16 | 17 | function [sat_clk_drift,sat_clk_rel_error]= eval_sat_clock_offset(gps_sat,sv_id,t_sv) 18 | 19 | mu = 3.986005e14; 20 | c = 2.99792458e8; 21 | F = -2*sqrt(mu)/c^2; 22 | delta_n = 4.908419e-9; 23 | t_oe = 147456.0000; 24 | 25 | n_0 = sqrt(mu/(gps_sat(sv_id).sqrt_a)^6); % (rad/s) 26 | t_k=t_sv-t_oe; % Time from eph ref epoch (s) 27 | n = n_0 + delta_n; % Corrected mean motion (rad/s) 28 | M_k = gps_sat(sv_id).M_0+n*t_k; % Mean anomaly (rad/s) 29 | 30 | % Perform Newton-Raphson solution for E_k estimate 31 | E_k= newton_raphson(gps_sat(sv_id).e,M_k); % Eccentric anomaly ESTIMATE for computing delta_tr 32 | 33 | sat_clk_rel_error = F*gps_sat(sv_id).e*gps_sat(sv_id).sqrt_a*sin(E_k); % relativistic clock correction in seconds 34 | 35 | sat_clk_drift = gps_sat(sv_id).af0 + gps_sat(sv_id).af1*(t_sv - t_oe); %drift in in seconds 36 | 37 | end 38 | 39 | -------------------------------------------------------------------------------- /eval_sbas_correction.m: -------------------------------------------------------------------------------- 1 | function [sbas_correction] = eval_sbas_correction(gps_time,gps_sat,inres_pos_data,user_pos_ecef) 2 | 3 | c = 2.99792458e8;% speed of light (m/s) 4 | 5 | for i = 1:20 6 | 7 | inres_ecef = latlong_to_ecef(inres_pos_data(i)); 8 | d(i) = compute_distance(user_pos_ecef,inres_ecef); 9 | end; 10 | 11 | temp = min(d); 12 | 13 | for j = 1:20 14 | 15 | if d(j) == min(d) 16 | closest_inres = j; 17 | end; 18 | end; 19 | 20 | closest_inres_pos_ecef = latlong_to_ecef(inres_pos_data(closest_inres)); 21 | closest_inres_pos_geodetic = inres_pos_data(closest_inres); 22 | 23 | N = 0; %no of visible sats 24 | 25 | for sv_id=1:31 % space vehicle number 26 | 27 | [sv_x,sv_y,sv_z] = calc_sat_pos_ecef(gps_sat,gps_time,sv_id); % The true position of satellite based on ephemeris data and the gps time 28 | 29 | true_sat_pos_ecef = struct('x',sv_x,'y',sv_y,'z',sv_z); 30 | 31 | [elev,azim,is_visible] = eval_el_az(closest_inres_pos_geodetic,closest_inres_pos_ecef,true_sat_pos_ecef); % gives elevation and azim in radians 32 | 33 | if is_visible == 1 34 | 35 | N = N +1; 36 | iono_delay(N) = eval_delay_iono(closest_inres_pos_geodetic,elev,azim,gps_time); 37 | tropo_delay(N) = eval_delay_tropo(elev,closest_inres_pos_geodetic.alt); 38 | 39 | end; 40 | 41 | end; 42 | 43 | iono_correction = mean(iono_delay);% + std(iono_delay); 44 | tropo_correction = mean(tropo_delay);% + std(tropo_delay); 45 | sbas_correction = c*(iono_correction + tropo_correction);% delay correction in metres 46 | 47 | end 48 | -------------------------------------------------------------------------------- /eval_state_derivative.asv: -------------------------------------------------------------------------------- 1 | % FUNCTION TO SOLVE STATE EQUATIONS 2 | function[DX,Acceleration_body] = eval_state_derivative(X,dth,de,da,dr) 3 | 4 | %** ADDITIONAL PARAMTERS FOR CALCULATION **% 5 | rtd = 57.29577951; % radian to degree conversion factor 6 | dtr = 0.017453293; % degree to radian conversion factor 7 | Pi = 3.141592654; 8 | Re = 6378137; 9 | OMEGA_dot_e = 7.2921151467e-5;%earth's rotation rate (rad/sec) 10 | e = 0.081819108426; 11 | 12 | pos_body_geo = struct('lat',X(10),'long',X(11),'alt',X(12)); 13 | 14 | C_b2n = [cos(X(5))*cos(X(6)) -cos(X(4))*sin(X(6))+sin(X(4))*sin(X(5))*cos(X(6)) sin(X(4))*sin(X(6))+cos(X(4))*sin(X(5))*cos(X(6));cos(X(5))*sin(X(6)) cos(X(4))*cos(X(6))+sin(X(4))*sin(X(5))*sin(X(6)) -sin(X(4))*cos(X(6))+cos(X(4))*sin(X(5))*sin(X(6));-sin(X(5)) sin(X(4))*cos(X(5)) cos(X(4))*cos(X(5))]; 15 | 16 | R_N = Re*(1-e^2)/(1-e^2*(sin(X(10)))^2)^1.5; 17 | 18 | R_E = Re/(1-e^2*(sin(X(10)))^2)^0.5; 19 | 20 | w_ien = OMEGA_dot_e*[cos(X(10));0;-sin(X(10))]; 21 | 22 | w_enn = [X(2)/(R_E+X(12));-X(1)/(R_N+X(12));-X(2)*tan(X(10))/(R_E+X(12))]; 23 | 24 | % gravitational acceleration 25 | gaccel_ned = eval_gaccel_ned(pos_body_geo); 26 | gaccel_body = C_b2n'*gaccel_ned; 27 | 28 | % Corriolis Force 29 | Fcor_ned = -OMEGA_dot_e^2*((Re+X(12))/2)*[sin(2*X(10));0;1+cos(2*X(10))]; %pg 48. tittterton strapdown intertial navigation 30 | Fcor_body = C_b2n'*Fcor_ned; 31 | 32 | g_ln = 33 | % Aircraft Parameters 34 | mass = 190;% [kg] 35 | Ixx = 47.22;% [kg-m^2] 36 | Iyy = 90.84;% [kg-m^2] 37 | Izz = 111.48;% [kg-m^2] 38 | Ixz = -6.64;% [kg-m^2] 39 | Sref = 2.826; % Reference Surface Area [m^2] 40 | bref = 5.15;% Reference Span [m] 41 | cref = 0.55;% Reference Chord Length [m] 42 | xcg = 1; %?? CG location relative to wing leading edge, expressed as a fraction of aerodynamic chord length 43 | heng = 0; % ??? Engine angular momentum, assumed fixed 44 | 45 | 46 | % Easy representation of moments of interia 47 | Gamma = Ixx * Izz - (Ixz * Ixz); 48 | C1 = ((Iyy - Izz) * Izz - (Ixz * Ixz))/ Gamma; 49 | C2 = ((Ixx - Iyy + Izz ) * Ixz ) / Gamma; 50 | C3 = Izz / Gamma; 51 | C4 = Ixz / Gamma; 52 | C5 = (Izz - Ixx) / Iyy; 53 | C6 = Ixz / Iyy ; 54 | C7 = 1 / Iyy; 55 | C8 = (Ixx * (Ixx - Iyy ) + Ixz * Ixz) / Gamma; 56 | C9 = Ixx / Gamma; 57 | 58 | Vbody = C_b2n'*[X(1);X(2);X(3)]; % convert velocity from ned to body frame to calculate aerodynamic forces 59 | 60 | Vt = norm(Vbody); 61 | BETA = rtd * asin(Vbody(2)/Vt); 62 | ALPHA = rtd * atan(Vbody(3)/Vbody(1)); 63 | DX = linspace(0,0,12); 64 | [qbar,rho] = atmosphere(X(12),Vt); 65 | Thrust = eval_Thrust(rho,Vt,dth); 66 | CD_fa = eval_CD_fa(ALPHA); 67 | CD_fade = eval_CD_fade(de,ALPHA); 68 | CL_fa = eval_CL_fa(ALPHA); 69 | CL_fade = eval_CL_fade(de,ALPHA); 70 | CY_fbdr = eval_CY_fbdr(dr,BETA); 71 | Cl_fada = eval_Cl_fada(da,ALPHA); 72 | Cm_fa = eval_Cm_fa(ALPHA); 73 | Cm_fade = eval_Cm_fade(de,ALPHA); 74 | Cn_fbdr = eval_Cn_fbdr(dr,BETA); 75 | Cn_fada = eval_Cn_fada(da,ALPHA); 76 | 77 | 78 | %Total force coefficients 79 | 80 | % Cx_tot 81 | CX_tot = CD_fa + CD_fade; 82 | 83 | % Cy_tot 84 | CY_tot = CY_fbdr; 85 | 86 | %/ Cz_tot 87 | CZ_tot = CL_fa + CL_fade; 88 | 89 | 90 | % Total moment coefficients 91 | 92 | % Cl_tot 93 | Cl_tot = Cl_fada ; 94 | 95 | %/ Cm_tot 96 | Cm_tot = Cm_fa + Cm_fade; 97 | 98 | % Cn_tot 99 | Cn_tot = Cn_fbdr + Cn_fada; 100 | 101 | 102 | % Total forces 103 | Xbar = qbar * Sref * CX_tot; 104 | Ybar = qbar * Sref * CY_tot; 105 | Zbar = qbar * Sref * CZ_tot; 106 | 107 | % Total moments 108 | Lbar = Cl_tot * qbar * Sref * bref; 109 | Mbar = Cm_tot * qbar * Sref * cref; 110 | Nbar = Cn_tot * qbar * Sref * bref; 111 | 112 | F_Aerod_body = [(X(9)*Vbody(2) - X(8)*Vbody(3) + (Thrust - Xbar)/mass);(-X(9)*Vbody(1) + X(7)*Vbody(3) - Ybar/mass);(X(8)*Vbody(1) - X(7)*Vbody(2) - Zbar/mass)]; 113 | 114 | F_Aerod_ned = C_b2n*F_Aerod_body; 115 | 116 | Acceleration_body = F_Aerod_body'; 117 | 118 | Fned = F_Aerod_ned - cross((2*w_ien + w_enn),[X(1);X(2);X(3)]) + g_ln; 119 | 120 | % Forces along body axis 121 | DX(1) = Fned(1); 122 | 123 | DX(2) = Fned(2); 124 | 125 | DX(3) = Fned(3); 126 | 127 | % Rate of change of euler angles w.r.t NED frame 128 | DX(4) = X(7) + tan(X(5))*(X(8)*sin(X(4)) + X(9)*cos(X(4))); 129 | 130 | DX(5) = X(8)*cos(X(4)) - X(9)*sin(X(4)); 131 | 132 | DX(6) = (X(8)*sin(X(4)) + X(9)*cos(X(4))) / cos(X(5)); 133 | 134 | % Angular acceleration 135 | DX(7) = (C1*X(9) + C2*X(7))*X(8) + C3*Lbar + C4*(Nbar + X(8) * heng); 136 | 137 | DX(8) = C5*X(7)*X(9) - C6*( X(7)^2 - X(9)^2) + C7*(Mbar - heng * X(9)); 138 | 139 | DX(9) = (C8*X(7) - C2*X(9))*X(8) + C4*Lbar + C9 * (Nbar + X(8) * heng); 140 | 141 | % Ldot,ldot,hdot 142 | DX(10) = X(1)/(R_N+X(12)); 143 | 144 | DX(11) = X(2)*sec(X(10))/(R_E+X(12)); 145 | 146 | DX(12) = -X(3); 147 | 148 | 149 | end -------------------------------------------------------------------------------- /eval_state_derivative.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Pioneer UAV Flight Mechanics Model 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | % References: 9 | % 1. "Aircraft Control and Simulation", B.L Stevenson & Frank M. Lewis 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 11 | % Outputs: 12 | % 1. DX: array containing derivative of state vector 13 | % 2. Acceleration_body: acceleration along body axis 14 | % 3. AngV : Angular velocity of aircraft about body axis 15 | % Inputs: 16 | % 1. X: state vector 17 | % 2. dth: throttle (95-500) 18 | % 3. de: elevator deflection (degrees) 19 | % 4. da: aileron deflection (degrees) 20 | % 5. dr: rudder deflection (degrees) 21 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 22 | function[DX,Acceleration_body,AngV] = eval_state_derivative(X,dth,de,da,dr) 23 | 24 | %** ADDITIONAL PARAMTERS FOR CALCULATION **% 25 | rtd = 57.29577951; % radian to degree conversion factor 26 | dtr = 0.017453293; % degree to radian conversion factor 27 | Pi = 3.141592654; 28 | Re = 6378137; % WGS-84 equatorial radius (m). 29 | OMEGA_dot_e = 7.292115e-5;%earth's rotation rate (rad/sec) 30 | f = 1/298.257223563; % WGS-84 Flattening. 31 | e = sqrt(f*(2 - f)); % Eccentricity. 32 | 33 | pos_body_geo = struct('lat',X(10),'long',X(11),'alt',X(12)); 34 | 35 | C_b2n = [cos(X(5))*cos(X(6)) -cos(X(4))*sin(X(6))+sin(X(4))*sin(X(5))*cos(X(6)) sin(X(4))*sin(X(6))+cos(X(4))*sin(X(5))*cos(X(6));cos(X(5))*sin(X(6)) cos(X(4))*cos(X(6))+sin(X(4))*sin(X(5))*sin(X(6)) -sin(X(4))*cos(X(6))+cos(X(4))*sin(X(5))*sin(X(6));-sin(X(5)) sin(X(4))*cos(X(5)) cos(X(4))*cos(X(5))]; 36 | 37 | R_N = Re*(1-e^2)/(1-e^2*(sin(X(10)))^2)^1.5; 38 | 39 | R_E = Re/(1-e^2*(sin(X(10)))^2)^0.5; 40 | 41 | w_ien = OMEGA_dot_e*[cos(X(10));0;-sin(X(10))]; 42 | 43 | w_enn = [X(2)/(R_E+X(12));-X(1)/(R_N+X(12));-X(2)*tan(X(10))/(R_E+X(12))]; 44 | 45 | % gravitational acceleration 46 | gaccel_ned = eval_gaccel_ned(pos_body_geo); 47 | g_ln = gaccel_ned - OMEGA_dot_e^2*((Re+X(12))/2)*[sin(2*X(10));0;1+cos(2*X(10))]; 48 | 49 | 50 | % Aircraft Parameters 51 | mass = 190;% [kg] 52 | Ixx = 47.22;% [kg-m^2] 53 | Iyy = 90.84;% [kg-m^2] 54 | Izz = 111.48;% [kg-m^2] 55 | Ixz = -6.64;% [kg-m^2] 56 | Sref = 2.826; % Reference Surface Area [m^2] 57 | bref = 5.15;% Reference Span [m] 58 | cref = 0.55;% Reference Chord Length [m] 59 | xcg = 1; %?? CG location relative to wing leading edge, expressed as a fraction of aerodynamic chord length 60 | heng = 0; % ??? Engine angular momentum, assumed fixed 61 | 62 | 63 | % Easy representation of moments of interia 64 | Gamma = Ixx * Izz - (Ixz * Ixz); 65 | C1 = ((Iyy - Izz) * Izz - (Ixz * Ixz))/ Gamma; 66 | C2 = ((Ixx - Iyy + Izz ) * Ixz ) / Gamma; 67 | C3 = Izz / Gamma; 68 | C4 = Ixz / Gamma; 69 | C5 = (Izz - Ixx) / Iyy; 70 | C6 = Ixz / Iyy ; 71 | C7 = 1 / Iyy; 72 | C8 = (Ixx * (Ixx - Iyy ) + Ixz * Ixz) / Gamma; 73 | C9 = Ixx / Gamma; 74 | 75 | Vbody = C_b2n'*[X(1);X(2);X(3)]; % convert velocity from ned to body frame to calculate aerodynamic forces 76 | 77 | Vt = norm(Vbody); 78 | BETA = rtd * asin(Vbody(2)/Vt); 79 | ALPHA = rtd * atan(Vbody(3)/Vbody(1)); 80 | DX = linspace(0,0,12); 81 | [qbar,rho] = atmosphere(X(12),Vt); 82 | Thrust = eval_Thrust(rho,Vt,dth); 83 | [CD_fa,CD_fade,CL_fa,CL_fade,CY_fbdr,Cl_fada,Cm_fa,Cm_fade,Cn_fbdr,Cn_fada] = eval_aerod_coeff(de,da,dr,ALPHA,BETA); 84 | 85 | %Total force coefficients 86 | 87 | % Cx_tot 88 | CX_tot = CD_fa + CD_fade; 89 | 90 | % Cy_tot 91 | CY_tot = CY_fbdr; 92 | 93 | %/ Cz_tot 94 | CZ_tot = CL_fa + CL_fade; 95 | 96 | 97 | % Total moment coefficients 98 | 99 | % Cl_tot 100 | Cl_tot = Cl_fada ; 101 | 102 | %/ Cm_tot 103 | Cm_tot = Cm_fa + Cm_fade; 104 | 105 | % Cn_tot 106 | Cn_tot = Cn_fbdr + Cn_fada; 107 | 108 | 109 | % Total forces 110 | Xbar = qbar * Sref * CX_tot; 111 | Ybar = qbar * Sref * CY_tot; 112 | Zbar = qbar * Sref * CZ_tot; 113 | 114 | % Total moments 115 | Lbar = Cl_tot * qbar * Sref * bref; 116 | Mbar = Cm_tot * qbar * Sref * cref; 117 | Nbar = Cn_tot * qbar * Sref * bref; 118 | 119 | F_Aerod_body = [(X(9)*Vbody(2) - X(8)*Vbody(3) + (Thrust - Xbar)/mass);(-X(9)*Vbody(1) + X(7)*Vbody(3) - Ybar/mass);(X(8)*Vbody(1) - X(7)*Vbody(2) - Zbar/mass)]; 120 | 121 | F_Aerod_ned = C_b2n*F_Aerod_body; 122 | 123 | Fned = F_Aerod_ned - cross((2*w_ien + w_enn),[X(1);X(2);X(3)]) + g_ln; 124 | 125 | % Forces along NED axis 126 | DX(1) = Fned(1); 127 | 128 | DX(2) = Fned(2); 129 | 130 | DX(3) = Fned(3); 131 | 132 | % Rate of change of euler angles w.r.t NED frame 133 | DX(4) = X(7) + tan(X(5))*(X(8)*sin(X(4)) + X(9)*cos(X(4))); 134 | 135 | DX(5) = X(8)*cos(X(4)) - X(9)*sin(X(4)); 136 | 137 | DX(6) = (X(8)*sin(X(4)) + X(9)*cos(X(4))) / cos(X(5)); 138 | 139 | % Angular acceleration 140 | DX(7) = (C1*X(9) + C2*X(7))*X(8) + C3*Lbar + C4*(Nbar + X(8) * heng); 141 | 142 | DX(8) = C5*X(7)*X(9) - C6*( X(7)^2 - X(9)^2) + C7*(Mbar - heng * X(9)); 143 | 144 | DX(9) = (C8*X(7) - C2*X(9))*X(8) + C4*Lbar + C9 * (Nbar + X(8) * heng); 145 | 146 | % Ldot,ldot,hdot 147 | DX(10) = X(1)/(R_N+X(12)); 148 | 149 | DX(11) = X(2)*sec(X(10))/(R_E+X(12)); 150 | 151 | DX(12) = -X(3); 152 | 153 | AngV = [X(7) X(8) X(9)]; 154 | Acceleration_body = F_Aerod_body'; 155 | end -------------------------------------------------------------------------------- /gaccel_e.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Calculates the gravitational acceleration given the ECEF position 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | % References: 9 | % Page 41, Aircraft Control and Simulation by Stevens & Lewis 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 11 | function gaccel_body = eval_gaccel_body(pos_body_ecef, pos_body_geo, phi,theta,psi) 12 | 13 | mu = 3.986004415e14; %m^3/s^2 14 | a = 6378137; %m; equatorial radius of earth 15 | % b = 6356752.3142; %m; polar radius of earth 16 | J2 = 0.00108263; 17 | 18 | r1 = pos_body_ecef(1); 19 | r2 = pos_body_ecef(2); 20 | r3 = pos_body_ecef(3); 21 | r = sqrt(r1^2+r2^2+r3^2); 22 | Lg = r3/r; %geocentric latitude 23 | 24 | gaccel_ECEF = -mu/r^2*... 25 | [(1+1.5*J2*(a/r)^2*(1-5*(Lg)^2))*r1/r;... 26 | (1+1.5*J2*(a/r)^2*(1-5*(Lg)^2))*r2/r;... 27 | (1+1.5*J2*(a/r)^2*(3-5*(Lg)^2))*r3/r]; 28 | end 29 | -------------------------------------------------------------------------------- /gps_alamanac_data.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Alamanac Data File % 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | % From http://www.navcen.uscg.gov/?pageName=gpsAlmanacs (data taken from site on 11th Oct, 2010) GPS Week 581 % 9 | % This file contains the alamanac data obtained for all 31 satellites % 10 | % We will use this file to load ephemris data % 11 | % for satellite position calculation % 12 | % We will use array structure to store the data % 13 | % the structure is of type (ephemeris reference time (s), eccentricity, % 14 | % inclination(rad), rate of right ascension(rad/s), sqrt(semi-major-axis in meter), right % 15 | % ascension at week(rad), argperigee (rad), mean anomaly (rad), af0, af1) % 16 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5%%%%%%%% 17 | % sv : space vehicle % 18 | 19 | function [gps_sat] = gps_alamanac_data(); 20 | sv1 = struct('e',0.9717941284E-002,'i_0',0.9399662018,'Omega_dot', -0.7930793799E-008,'sqrt_a', 5153.591797,'Omega_0',0.3129077792E+001,'omega',3.140386343,'M_0',0.1648490429E+001,'af0',0.2946853638E-003,'af1',0.3637978807E-011); 21 | 22 | sv2 = struct('e',0.1375055313E-001,'i_0',0.9274730682,'Omega_dot',-0.8283677744E-008,'sqrt_a', 5153.542969,'Omega_0',0.1974781156e+001,'omega',1.046698451,'M_0',0.1529784799E+001,'af0',0.6198883057E-003,'af1',0.3637978807E-011); 23 | 24 | sv3 = struct('e',0.9379386902E-002,'i_0',0.9390869141,'Omega_dot',-0.7927155821E-008,'sqrt_a', 5153.706055,'Omega_0', -0.3136425257E+001,'omega', 0.654833794,'M_0',-0.1631534219E+001,'af0',0.1916885376E-003,'af1',0.1091393642E-010); 25 | 26 | sv4 = struct('e',0.1934528351E-002,'i_0', 0.9566497803,'Omega_dot',-0.7898051990E-008,'sqrt_a', 5153.491211,'Omega_0',-0.2093719006E+001,'omega', 0.150305748,'M_0',-0.2951365232E+001,'af0', -0.4959106445E-004,'af1',-0.7275957614E-011); 27 | 28 | sv5 = struct('e',0.6158351898E-002,'i_0', 0.9347667694,'Omega_dot',-0.8218194125E-008,'sqrt_a', 5153.587402,'Omega_0',0.2046554685E+001,'omega', -0.937443852,'M_0',-0.2582926273E+001,'af0', 0.4596710205E-003,'af1',-0.1455191523E-010); 29 | 30 | sv6 = struct('e',0.4140377045E-002,'i_0', 0.9731807709,'Omega_dot',-0.7818016456E-008,'sqrt_a', 5153.645996,'Omega_0',0.8900165558E-002,'omega', 3.116469979,'M_0',-0.4353499413E-001,'af0', -0.1907348633E-005,'af1',0.0000000000E+000); 31 | 32 | sv7 = struct('e',0.1152896881E-001,'i_0', 0.9946689606,'Omega_dot',-0.7556081982E-008,'sqrt_a', 5153.620117,'Omega_0',0.7961344719E-001,'omega', -3.134567976,'M_0',-0.6259098053E+000,'af0', 0.6675720215E-005,'af1',0.0000000000E+000); 33 | 34 | sv8 = struct('e',0.1700687408E-001,'i_0', 0.9822177887,'Omega_dot',-0.7748894859E-008,'sqrt_a', 5153.659668,'Omega_0',-0.2124357224E-001,'omega',1.552435040,'M_0',-0.7527018785E+000,'af0', 0.3623962402E-004,'af1',0.3637978807E-011); 35 | 36 | sv9 = struct('e',0.9441375732E-002,'i_0', 0.9516220093,'Omega_dot',-0.7956259651E-008,'sqrt_a', 5153.479004,'Omega_0',-0.2068731189E+001,'omega',0.662930965,'M_0',-0.2810052395E+001,'af0', -0.5531311035E-004,'af1', 0.0000000000E+000); 37 | 38 | sv10 = struct('e',0.1132488251E-001,'i_0', 0.8885612488,'Omega_dot',-0.8516508387E-008,'sqrt_a', 5153.701660,'Omega_0',0.2903643250E+001,'omega',0.854365110,'M_0',-0.1704478264E+000,'af0', -0.9536743164E-004,'af1', -0.3637978807E-011); 39 | 40 | sv11 = struct('e',0.3305435181E-002,'i_0', 0.9721927643,'Omega_dot',-0.7632479537E-008,'sqrt_a', 5153.544434,'Omega_0',0.1062735558E+001,'omega',-0.324701667,'M_0',-0.4612469673E-001,'af0', -0.6484985352E-004,'af1', 0.3637978807E-011); 41 | 42 | sv12 = struct('e', 0.4311084747E-002,'i_0', 0.9916305542,'Omega_dot',-0.7916241884E-008,'sqrt_a', 5153.650879,'Omega_0',-0.9695185423E+000,'omega',1.808801174,'M_0',0.2388894558E+001,'af0', 0.2956390381E-003,'af1', 0.0000000000E+000); 43 | 44 | sv13 = struct('e', 0.5702972412E-002,'i_0', 0.9847888947,'Omega_dot',-0.7989001460E-008,'sqrt_a', 5153.534668,'Omega_0',-0.9922137260E+000,'omega',-2.112283349,'M_0',0.2341084242E+001,'af0', 0.9727478027E-004,'af1', 0.3637978807E-011); 45 | 46 | sv14 = struct('e', 0.2831935883E-002,'i_0', 0.9521846771,'Omega_dot',-0.8305505617E-008,'sqrt_a', 5153.774414,'Omega_0', -0.1060687423E+001,'omega', -0.210268855,'M_0',0.2191159725E+001,'af0',-0.1974105835E-003,'af1', 0.3637978807E-011); 47 | 48 | sv15 = struct('e', 0.5815982819E-002,'i_0', 0.9733123779,'Omega_dot',-0.7657945389E-008,'sqrt_a', 5153.625000,'Omega_0', 0.1080329537E+001,'omega', -0.160482645,'M_0', -0.2377792358E+001,'af0',-0.1163482666E-003,'af1', -0.3637978807E-011); 49 | 50 | sv16 = struct('e', 0.5634307861E-002,'i_0', 0.9603576660,'Omega_dot',-0.7941707736E-008,'sqrt_a', 5153.605957,'Omega_0', 0.2123745799E+001,'omega', -2.512104392,'M_0', 0.2874350429E+001,'af0',0.1735687256E-003,'af1', 0.0); 51 | 52 | sv17 = struct('e', 0.1185274124E-001,'i_0', 0.9355697632,'Omega_dot', -0.8072674973E-008,'sqrt_a', 5153.682617,'Omega_0', -0.2070865870E+001,'omega', -2.287830114,'M_0',-0.2064763665E+001,'af0',0.9822845459E-004,'af1',0.3637978807E-011); 53 | 54 | sv18 = struct('e', 0.6700038910E-002,'i_0', 0.9579792023,'Omega_dot', -0.7963535609E-008,'sqrt_a', 5153.515137,'Omega_0', 0.2178431392E+001,'omega', -0.045089126,'M_0', 0.2020302892E+001,'af0',-0.7343292236E-004,'af1',-0.3637978807E-011); 55 | 56 | sv19 = struct('e', 0.4257202148E-002,'i_0', 0.9356040955,'Omega_dot', -0.8076312952E-008,'sqrt_a', 5153.761719,'Omega_0', -0.2124836087E+001,'omega', 1.266663313,'M_0', -0.1680142999E+001,'af0',0.5054473877E-004,'af1',0); 57 | 58 | sv20 = struct('e', 0.1688814163E-001,'i_0', 0.9326629639,'Omega_dot', -0.8036295185E-008,'sqrt_a', 5153.568848,'Omega_0', -0.3126577973E+001,'omega', -2.439979553,'M_0', -0.8855537176E+000,'af0',-0.9441375732E-004,'af1',-0.3637978807E-011); 59 | 60 | sv21 = struct('e', 0.5580425262E-002,'i_0', 0.9332618713,'Omega_dot', -0.8116330719E-008,'sqrt_a', 5153.667969,'Omega_0', -0.2066244841E+001,'omega', -1.927601099,'M_0', -0.2963628054E+001,'af0',0.1611709595E-003,'af1',0); 61 | 62 | sv22 = struct('e', 0.6992340088E-002,'i_0', 0.9665241241,'Omega_dot', -0.8159986464E-008,'sqrt_a', 5153.583984,'Omega_0', -0.1033619881E+001,'omega', 3.114451051,'M_0', 0.1576552153E+001,'af0',0.3490447998E-003,'af1',-0.3637978807E-011); 63 | 64 | sv23 = struct('e', 0.5754470825E-002,'i_0', 0.9492797852,'Omega_dot', -0.7799826562E-008,'sqrt_a', 5154.707520,'Omega_0', -0.3085681319E+001,'omega', -0.359179974,'M_0', 0.1618582249E+001,'af0',0.3290176392E-003,'af1', 0.3637978807E-011); 65 | 66 | sv24 = struct('e',0.7176399231E-003,'i_0', 0.9609222412,'Omega_dot', -0.7752532838E-008,'sqrt_a', 5153.712891,'Omega_0', 0.1035565853E+001,'omega', 1.971091509,'M_0', -0.2850866199E+001,'af0',-0.4482269287E-004,'af1', -0.3637978807E-011); 67 | 68 | sv25 = struct('e',0.1984405518E-001,'i_0', 0.9893188477,'Omega_dot', -0.7959897630E-008,'sqrt_a', 5153.595215,'Omega_0', -0.9734778404E+000,'omega', 1.112707138,'M_0', 0.1301463962E+001,'af0',-0.2765655518E-004,'af1',-0.7275957614E-011); 69 | 70 | sv26 = struct('e',0.2162837982E-001,'i_0', 0.9798450470,'Omega_dot', -0.7734342944E-008,'sqrt_a', 5153.530273,'Omega_0', -0.4657363892E-001,'omega', -1.363448977,'M_0', 0.2437499166E+001,'af0',0.1974105835E-003,'af1',0.3637978807E-011); 71 | 72 | sv27 = struct('e',0.1626777649E-001,'i_0',0.9715690613,'Omega_dot', -0.7668859325E-008,'sqrt_a',5153.635254,'Omega_0', 0.1087866545E+001,'omega', -1.944749475,'M_0', -0.2811243415E+001,'af0',0,'af1',0); 73 | 74 | sv28 = struct('e',0.2580642700E-002,'i_0',0.9607295990,'Omega_dot', -0.7938069757E-008,'sqrt_a',5153.591797,'Omega_0', 0.2132056952E+001,'omega', -1.248912454,'M_0', -0.7631899118E+000,'af0',0.1554489136E-003,'af1',0.3637978807E-011); 75 | 76 | sv29 = struct('e',0.1312303543E-001,'i_0',0.9575901031,'Omega_dot', -0.7763446774E-008,'sqrt_a',5155.299805,'Omega_0', 0.1007025003E+001,'omega', 1.551597953,'M_0', -0.2585166574E+001,'af0',0.2861022949E-003,'af1',0.3637978807E-011); 77 | 78 | sv30 = struct('e',0.7449150085E-002,'i_0',0.9784011841,'Omega_dot',-0.7767084753E-008,'sqrt_a',5153.737305,'Omega_0', 0.9124159813E-002,'omega', -1.052870393,'M_0', -0.5640900135E-001,'af0',-0.1907348633E-005,'af1',0.3637978807E-011); 79 | 80 | sv31 = struct('e',0.1267480850E-001,'i_0',0.9573802948,'Omega_dot', -0.7901689969E-008,'sqrt_a',5153.561523,'Omega_0', -0.2004679561E+001,'omega', -0.917668819,'M_0', 0.7545526028E+000,'af0', -0.1058578491E-003,'af1',-0.7275957614E-011); 81 | 82 | sv32 = struct('e',1.521692e-2,'i_0',9.464037e-1,'Omega_dot', -8.464281e-9,'sqrt_a',5.153688e+003,'Omega_0', 1.384688,'omega', -2.466272,'M_0', -3.682930e-1,'af0', -2.502208e-4,'af1',-3.183231e-12); 83 | % sv32 is test value taken from grewals matlab file 84 | 85 | % Consolidate all these arrars into 1 array called gps_sat % 86 | gps_sat = [sv1,sv2,sv3,sv4,sv5,sv6,sv7,sv8,sv9,sv10,sv11,sv12,sv13,sv14,sv15,sv16,sv17,sv18,sv19,sv20,sv21,sv22,sv23,sv24,sv25,sv26,sv27,sv28,sv29,sv30,sv31,sv32]; 87 | end -------------------------------------------------------------------------------- /inres_pos.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Global Positioning System Simulation Matlab Tool 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | %%% This function contains the location of the indian reference stations 9 | 10 | function [list_inres] = inres_pos_data() 11 | 12 | inres_1 = struct('lat',28.55*dtr,'long',77.1*dtr,'alt',216); %delhi 13 | inres_2 = struct('lat',32.683*dtr,'long',74.233,'alt',314); %jammu 14 | inres_3 = struct('lat',23.066*dtr,'long',38.066*dtr,'alt',58); %ahmedabad 15 | inres_4 = struct('lat',26.25*dtr,'long',73.02*dtr,'alt',219); %jodhpur 16 | inres_5 = struct('lat',26.75*dtr,'long',77.33*dtr,'alt',125); % lucknow 17 | inres_6 = struct('lat',23.28*dtr,'long',77.33*dtr,'alt',524);%bhopal 18 | inres_7 = struct('lat',26.67*dtr,'long',88.33*dtr,'alt',126); %bagdogra 19 | inres_8 = struct('lat',26.1*dtr,'long',91.5833*dtr,'alt',49);%guwahai 20 | inres_9 = struct('lat',23.833*dtr,'long',92.62*dtr,'alt',405); % aizwal 21 | inres_10 = struct('lat',27.5*dtr,'long',95*dtr,'alt',110);%dibrugarh 22 | inres_11 = struct('lat',21.17*dtr,'long',81.73*dtr,'alt',317);%raipur 23 | inres_12 = struct('lat',22.65*dtr,'long',88.433*dtr,'alt',5); %kolkata 24 | inres_13 = struct('lat',19.083*dtr,'long',72.86*dtr,'alt',11);%mumbai 25 | inres_14 = struct('lat',17.28*dtr,'long',78.42*dtr,'alt',617);%hyderabad 26 | inres_15 = struct('lat',17.72*dtr,'long',83.22*dtr,'alt',5); %vishakhapatnam 27 | inres_16 = struct('lat',13.20*dtr,'long',77.7*dtr,'alt',915); %bangalore 28 | inres_17 = struct('lat',13*dtr,'long',9.15*dtr,'alt',16); %chennai 29 | inres_18 = struct('lat',10.82*dtr,'long',72.17*dtr,'alt',4); %agatti 30 | inres_19 = struct('lat',8.5*dtr,'long',76.92*dtr,'alt',4); %trivandrum 31 | inres_20 = struct('lat',11.633*dtr,'long',92.72*dtr,'alt',4);%port blair 32 | 33 | list_inres = [inres_1,inres_2,inres_3,inres_4inres_5,inres_6,inres_7,inres_8,inres_9,inres_10,inres_11,inres_12,inres_13,inres_14,inres_15,inres_16,inres_17,inres_18,inres_19,inres_20]; 34 | end 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /inres_pos_data.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Global Positioning System Simulation Matlab Tool 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | %%% This function contains the location of the indian reference stations 9 | 10 | function [list_inres] = inres_pos_data() 11 | rtd = 180/3.14159; % radians to degree 12 | dtr = 3.14159/180; 13 | inres_1 = struct('lat',28.55*dtr,'long',77.1*dtr,'alt',216); %delhi 14 | inres_2 = struct('lat',32.683*dtr,'long',74.233,'alt',314); %jammu 15 | inres_3 = struct('lat',23.066*dtr,'long',38.066*dtr,'alt',58); %ahmedabad 16 | inres_4 = struct('lat',26.25*dtr,'long',73.02*dtr,'alt',219); %jodhpur 17 | inres_5 = struct('lat',26.75*dtr,'long',77.33*dtr,'alt',125); % lucknow 18 | inres_6 = struct('lat',23.28*dtr,'long',77.33*dtr,'alt',524);%bhopal 19 | inres_7 = struct('lat',26.67*dtr,'long',88.33*dtr,'alt',126); %bagdogra 20 | inres_8 = struct('lat',26.1*dtr,'long',91.5833*dtr,'alt',49);%guwahai 21 | inres_9 = struct('lat',23.833*dtr,'long',92.62*dtr,'alt',405); % aizwal 22 | inres_10 = struct('lat',27.5*dtr,'long',95*dtr,'alt',110);%dibrugarh 23 | inres_11 = struct('lat',21.17*dtr,'long',81.73*dtr,'alt',317);%raipur 24 | inres_12 = struct('lat',22.65*dtr,'long',88.433*dtr,'alt',5); %kolkata 25 | inres_13 = struct('lat',19.083*dtr,'long',72.86*dtr,'alt',11);%mumbai 26 | inres_14 = struct('lat',17.28*dtr,'long',78.42*dtr,'alt',617);%hyderabad 27 | inres_15 = struct('lat',17.72*dtr,'long',83.22*dtr,'alt',5); %vishakhapatnam 28 | inres_16 = struct('lat',13.20*dtr,'long',77.7*dtr,'alt',915); %bangalore 29 | inres_17 = struct('lat',13*dtr,'long',9.15*dtr,'alt',16); %chennai 30 | inres_18 = struct('lat',10.82*dtr,'long',72.17*dtr,'alt',4); %agatti 31 | inres_19 = struct('lat',8.5*dtr,'long',76.92*dtr,'alt',4); %trivandrum 32 | inres_20 = struct('lat',11.633*dtr,'long',92.72*dtr,'alt',4);%port blair 33 | 34 | list_inres = [inres_1,inres_2,inres_3,inres_4,inres_5,inres_6,inres_7,inres_8,inres_9,inres_10,inres_11,inres_12,inres_13,inres_14,inres_15,inres_16,inres_17,inres_18,inres_19,inres_20]; 35 | end 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /integ_RK4.asv: -------------------------------------------------------------------------------- 1 | % RK4 INTEGRATION FUNCTION 2 | function[Xnew,DX,Acceleration_body] = integ_RK4(dt,X,dth,de,da,dr); 3 | 4 | XA = linspace(0,0,12); 5 | XB = XA; 6 | [DX,Acceleration_body] = eval_state_derivative(X,dth,de,da,dr); 7 | XA = DX * dt; 8 | XB = X + 0.5 * XA; 9 | [DX,Acceleration_body] = eval_state_derivative(XB,dth,de,da,dr); 10 | Q = DX * dt; 11 | XB = X + 0.5 * Q; 12 | XA = XA + 2.0 * Q; 13 | [DX,Acceleration_body] = eval_state_derivative(XB,dth,de,da,dr); 14 | Q = DX * dt; 15 | XB = X + Q; 16 | XA = XA + 2.0 * Q; 17 | [DX,Acceleration_body] = eval_state_derivative(XB,dth,de,da,dr); 18 | Xnew = X + (XA + DX * dt)/6.0; 19 | end -------------------------------------------------------------------------------- /integ_RK4.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % RK4 INTEGRATION FUNCTION 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | function[Xnew,DX,Acceleration_body,AngV] = integ_RK4(dt,X,dth,de,da,dr); 9 | 10 | [DX,Acceleration_body(1,:),AngV(1,:)] = eval_state_derivative(X,dth,de,da,dr); 11 | XA = DX * dt; 12 | XB = X + 0.5 * XA; 13 | [DX,Acceleration_body(2,:),AngV(2,:)] = eval_state_derivative(XB,dth,de,da,dr); 14 | Q = DX * dt; 15 | XB = X + 0.5 * Q; 16 | XA = XA + 2.0 * Q; 17 | [DX,Acceleration_body(3,:),AngV(3,:)] = eval_state_derivative(XB,dth,de,da,dr); 18 | Q = DX * dt; 19 | XB = X + Q; 20 | XA = XA + 2.0 * Q; 21 | [DX,Acceleration_body(4,:),AngV(4,:)] = eval_state_derivative(XB,dth,de,da,dr); 22 | Xnew = X + (XA + DX * dt)/6.0; 23 | 24 | end -------------------------------------------------------------------------------- /landing_trajectory.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sauravag/GPSMATLAB/1549e13ccc5b7efe3d0ec6c3040d421c3b59d811/landing_trajectory.mat -------------------------------------------------------------------------------- /latlong_to_ecef.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Convert from Geodetic to ECEF coordinates 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | 9 | function [pos_ecef] = latlong_to_ecef(user_pos_geodetic); 10 | 11 | a = 6378137.0;% semi major axis in m 12 | 13 | e = sqrt(6.69437999014e-3); 14 | 15 | lat = user_pos_geodetic.lat; 16 | 17 | long = user_pos_geodetic.long; 18 | 19 | alt = user_pos_geodetic.alt; 20 | 21 | X = sqrt(1 - e^2*(sin(lat))^2); 22 | 23 | N = a/X; 24 | 25 | x = (N + alt)*cos(lat)*cos(long); 26 | 27 | y = (N + alt)*cos(lat)*sin(long); 28 | 29 | z = (N*(1-e^2)+ alt)*sin(lat); 30 | 31 | pos_ecef = struct('x',x,'y',y,'z',z); 32 | 33 | end 34 | -------------------------------------------------------------------------------- /ned_to_ecef.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Convert from Geodetic to ECEF coordinates 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | 9 | 10 | function [vector_ECEF] = ned_to_ecef(pos_body_ecef,vector) 11 | 12 | % C_b2n = [cos(theta)*cos(psi) -cos(phi)*sin(psi)+sin(phi)*sin(theta)*cos(psi) sin(phi)*sin(psi)+cos(phi)*sin(theta)*cos(psi);cos(theta)*sin(psi) cos(phi)*cos(psi)+sin(phi)*sin(theta)*sin(psi) -sin(phi)*cos(psi)+cos(phi)*sin(theta)*sin(psi);-sin(theta) sin(phi)*cos(theta) cos(phi)*cos(theta)]; 13 | 14 | pos_body_geo = ecef_to_latlong(pos_body_ecef); 15 | lat = pos_body_geo.lat; 16 | long = pos_body_geo.long; 17 | C_e2n = [-sin(lat)*cos(long) -sin(lat)*sin(long) cos(lat);-sin(long) cos(long) 0; -cos(lat)*cos(long) -cos(lat)*sin(long) -sin(lat)]; 18 | 19 | vector_ECEF = (C_e2n'*vector')'; 20 | 21 | end -------------------------------------------------------------------------------- /newton_raphson.m: -------------------------------------------------------------------------------- 1 | %%% function to perform newton raphson estimation %%%% 2 | 3 | function [Ecc_k] = newton_raphson(e,M_k); 4 | NRnext=0; 5 | NR=1; 6 | while abs(NRnext-NR)>1e-12; 7 | NR=NRnext; 8 | f=NR-e*sin(NR)-M_k; 9 | f1=1-e*cos(NR); 10 | f2=e*sin(NR); 11 | NRnext=NR-(f/(f1-(f2*f/2*f1))); 12 | end; 13 | Ecc_k=NRnext; % Eccentric anomaly ESTIMATE 14 | end -------------------------------------------------------------------------------- /rcvr_clk_model.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Clock bias function oscillator type: TCXO 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | function [rcvr_clock_error] = rcvr_clk_model(initial_bias,randomfactor) 9 | 10 | 11 | rcvr_clock_error = initial_bias + 2.5e-6 + 1e-6/(2*32768*365*24*3600) + 5e-8*randomfactor ; 12 | 13 | end -------------------------------------------------------------------------------- /select_optimum_sats.asv: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % This function selects the optimum satellites to track for best DOP 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | % References: 9 | % 1. Enge and Mishra: Global Positioning System 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 11 | 12 | function [optimum_sv_ids, min_GDOP,min_PDOP,min_HDOP,min_VDOP] = select_optimum_sats(gps_sat,gps_time,visible_sats_id,true_user_pos_ecef,initial_user_pos_estimate); 13 | 14 | xu = initial_user_pos_estimate.x; 15 | yu = initial_user_pos_estimate.y; 16 | zu = initial_user_pos_estimate.z; 17 | 18 | user_ecef_pos_estimate = struct('x',xu,'y',yu,'z',zu); 19 | 20 | select_4_sats = nchoosek(visible_sats_id,4); % compute all possible combinations of 4 satellites from the visible ones 21 | [rows = size(select_4_sats); 22 | 23 | for i = 1:rows 24 | 25 | for k = 1:4 26 | 27 | sv_id = select_4_sats(i,k); 28 | 29 | [sat_clk_drift,sat_clk_rel_error] = eval_sat_clock_offset(gps_sat,sv_id,gps_time); % satellite clock offset in seconds (clock drift + relativistic error) 30 | 31 | time_str = gps_time + sat_clk_drift + sat_clk_rel_error; % The time of tranmission from satellite as embedded in transmitted code 32 | 33 | [xs,ys,zs] = calc_sat_pos_ecef(gps_sat,time_str,sv_id); % The position of satellite based on ephemeris data and the time embedded in message 34 | 35 | computed_sat_pos_ecef = struct('x',xs,'y',ys,'z',zs); 36 | 37 | d = compute_distance(computed_sat_pos_ecef,user_ecef_pos_estimate); % what the receiver thinks is true pseduo range 38 | 39 | A(k,1) = -(xs - xu)/d; 40 | A(k,2) = -(ys - yu)/d ; 41 | A(k,3) = -(zs - zu)/d ; 42 | A(k,4) = -1; 43 | 44 | end; 45 | 46 | [GDOP(i), PDOP(i), HDOP(i), VDOP(i)] = eval_DOP(A); 47 | 48 | end; 49 | 50 | min_GDOP = min(GDOP); 51 | 52 | for l = 1:rows 53 | if GDOP(l) == min_GDOP 54 | optimum_sv_ids = select_4_sats(l,:); % the satellite geometry with the min DOP is selected for tracking 55 | min_PDOP = PDOP(l); 56 | min_HDOP = HDOP(l); 57 | min_VDOP = VDOP(l); 58 | end; 59 | end; 60 | 61 | end -------------------------------------------------------------------------------- /select_optimum_sats.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % This function selects the optimum satellites to track for best DOP 3 | % Author: Saurav Agarwal 4 | % Email: saurav6@gmail.com 5 | % Date: January 1, 2011 6 | % Place: Dept. of Aerospace Engg., IIT Bombay, Mumbai, India 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | % References: 9 | % 1. Enge and Mishra: Global Positioning System 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 11 | 12 | function [optimum_sv_ids, min_GDOP,min_PDOP,min_HDOP,min_VDOP] = select_optimum_sats(gps_sat,gps_time,visible_sats_id,true_user_pos_ecef,initial_user_pos_estimate); 13 | 14 | xu = initial_user_pos_estimate.x; 15 | yu = initial_user_pos_estimate.y; 16 | zu = initial_user_pos_estimate.z; 17 | 18 | user_ecef_pos_estimate = struct('x',xu,'y',yu,'z',zu); 19 | 20 | select_4_sats = nchoosek(visible_sats_id,4); % compute all possible combinations of 4 satellites from the visible ones 21 | rows = size(select_4_sats); 22 | 23 | for i = 1:rows 24 | 25 | for k = 1:4 26 | 27 | sv_id = select_4_sats(i,k); 28 | 29 | [sat_clk_drift,sat_clk_rel_error] = eval_sat_clock_offset(gps_sat,sv_id,gps_time); % satellite clock offset in seconds (clock drift + relativistic error) 30 | 31 | time_str = gps_time + sat_clk_drift + sat_clk_rel_error; % The time of tranmission from satellite as embedded in transmitted code 32 | 33 | [xs,ys,zs] = calc_sat_pos_ecef(gps_sat,time_str,sv_id); % The position of satellite based on ephemeris data and the time embedded in message 34 | 35 | computed_sat_pos_ecef = struct('x',xs,'y',ys,'z',zs); 36 | 37 | d = compute_distance(computed_sat_pos_ecef,user_ecef_pos_estimate); % what the receiver thinks is true pseduo range 38 | 39 | A(k,1) = -(xs - xu)/d; 40 | A(k,2) = -(ys - yu)/d ; 41 | A(k,3) = -(zs - zu)/d ; 42 | A(k,4) = -1; 43 | 44 | end; 45 | 46 | [GDOP(i), PDOP(i), HDOP(i), VDOP(i)] = eval_DOP(A); 47 | 48 | end; 49 | 50 | min_GDOP = min(GDOP); 51 | 52 | for l = 1:rows 53 | if GDOP(l) == min_GDOP 54 | optimum_sv_ids = select_4_sats(l,:); % the satellite geometry with the min DOP is selected for tracking 55 | min_PDOP = PDOP(l); 56 | min_HDOP = HDOP(l); 57 | min_VDOP = VDOP(l); 58 | end; 59 | end; 60 | 61 | end --------------------------------------------------------------------------------