Added files via GitHub desktop

Adjust_profile_position.m

@@ -0,0 +1,77 @@
+function Adjust_profile_position(in_filename,out_filename)
+%Adjust_profile_position - Adjusts the position in a motion profile file to
+% make it consistent with the velocity
+%
+% Software for use with "Principles of GNSS, Inertial, and Multisensor
+% Integrated Navigation Systems," Second Edition.
+%
+% This function created 2/4/2012 by Paul Groves
+%
+% Inputs
+%  in_filename     Name of inpit file, e.g. 'In_profile.csv'
+%  out_filename    Name of inpit file, e.g. 'Out_profile.csv'
+
+% Copyright 2012, Paul Groves
+% License: BSD; see license.txt for details
+
+% Begins
+
+% Parameters
+deg_to_rad = 0.01745329252;
+rad_to_deg = 1/deg_to_rad;
+
+
+% Input truth motion profile from .csv format file
+[in_profile,no_epochs,ok] = Read_profile(in_filename);
+
+% End script if there is a problem with the file
+if ~ok
+    return;
+end %if
+
+% Initialize navigation solution
+old_time = in_profile(1,1);
+old_L_b = in_profile(1,2);
+old_lambda_b = in_profile(1,3);
+old_h_b = in_profile(1,4);
+old_v_eb_n = in_profile(1,5:7)';
+old_eul_nb = in_profile(1,8:10)';
+out_profile(1,:) = in_profile(1,:);
+
+% Main loop
+for epoch = 2:no_epochs
+
+    % Input data from profile
+    time = in_profile(epoch,1);
+    v_eb_n = in_profile(epoch,5:7)';
+    eul_nb = in_profile(epoch,8:10)';
+
+    % Time interval
+    tor_i = time - old_time;
+
+    % Update position 
+    [L_b,lambda_b,h_b]= Update_curvilinear_position(tor_i,old_L_b,...
+    old_lambda_b,old_h_b,old_v_eb_n,v_eb_n);
+
+    % Generate output profile record
+    out_profile(epoch,1) = time;
+    out_profile(epoch,2) = L_b;
+    out_profile(epoch,3) = lambda_b;
+    out_profile(epoch,4) = h_b;
+    out_profile(epoch,5:7) = v_eb_n';
+    out_profile(epoch,8:10) = eul_nb';
+
+    % Reset old values
+    old_time = time;
+    old_L_b = L_b;
+    old_lambda_b = lambda_b;
+    old_h_b = h_b;
+    old_v_eb_n = v_eb_n;
+    old_eul_nb = eul_nb;
+
+end %epoch
+
+% Write output profile
+Write_profile(out_filename,out_profile);
+
+% Ends

Adjust_profile_velocity.m

@@ -0,0 +1,79 @@
+function Adjust_profile_velocity(in_filename,out_filename)
+%Adjust_profile_velocity - Adjusts the velocity in a motion profile file to
+% make it consistent with the position
+%
+% Software for use with "Principles of GNSS, Inertial, and Multisensor
+% Integrated Navigation Systems," Second Edition.
+%
+% This function created 2/4/2012 by Paul Groves
+%
+% Inputs
+%  in_filename     Name of inpit file, e.g. 'In_profile.csv'
+%  out_filename    Name of inpit file, e.g. 'Out_profile.csv'
+
+% Copyright 2012, Paul Groves
+% License: BSD; see license.txt for details
+
+% Begins
+
+% Parameters
+deg_to_rad = 0.01745329252;
+rad_to_deg = 1/deg_to_rad;
+
+
+% Input truth motion profile from .csv format file
+[in_profile,no_epochs,ok] = Read_profile(in_filename);
+
+% End script if there is a problem with the file
+if ~ok
+    return;
+end %if
+
+% Initialize navigation solution
+old_time = in_profile(1,1);
+old_L_b = in_profile(1,2);
+old_lambda_b = in_profile(1,3);
+old_h_b = in_profile(1,4);
+old_v_eb_n = in_profile(1,5:7)';
+old_eul_nb = in_profile(1,8:10)';
+out_profile(1,:) = in_profile(1,:);
+
+% Main loop
+for epoch = 2:no_epochs
+
+    % Input data from profile
+    time = in_profile(epoch,1);
+    L_b = in_profile(epoch,2);
+    lambda_b = in_profile(epoch,3);
+    h_b = in_profile(epoch,4);
+    eul_nb = in_profile(epoch,8:10)';
+
+    % Time interval
+    tor_i = time - old_time;
+
+    % Update velocity
+    v_eb_n = Velocity_from_curvilinear(tor_i,old_L_b,...
+    old_lambda_b,old_h_b,old_v_eb_n,L_b,lambda_b,h_b);
+
+    % Generate output profile record
+    out_profile(epoch,1) = time;
+    out_profile(epoch,2) = L_b;
+    out_profile(epoch,3) = lambda_b;
+    out_profile(epoch,4) = h_b;
+    out_profile(epoch,5:7) = v_eb_n';
+    out_profile(epoch,8:10) = eul_nb';
+
+    % Reset old values
+    old_time = time;
+    old_L_b = L_b;
+    old_lambda_b = lambda_b;
+    old_h_b = h_b;
+    old_v_eb_n = v_eb_n;
+    old_eul_nb = eul_nb;
+
+end %epoch
+
+% Write output profile
+Write_profile(out_filename,out_profile);
+
+% Ends

CTM_to_Euler.m

@@ -0,0 +1,28 @@
+function eul = CTM_to_Euler(C)
+%CTM_to_Euler - Converts a coordinate transformation matrix to the
+%corresponding set of Euler angles%
+%
+% Software for use with "Principles of GNSS, Inertial, and Multisensor
+% Integrated Navigation Systems," Second Edition.
+%
+% This function created 1/4/2012 by Paul Groves
+%
+% Inputs:
+%   C       coordinate transformation matrix describing transformation from
+%           beta to alpha
+%
+% Outputs:
+%   eul     Euler angles describing rotation from beta to alpha in the 
+%           order roll, pitch, yaw(rad)
+
+% Copyright 2012, Paul Groves
+% License: BSD; see license.txt for details
+
+% Begins
+
+% Calculate Euler angles using (2.23)
+eul(1,1) = atan2(C(2,3),C(3,3));  % roll
+eul(2,1) = - asin(C(1,3));        % pitch
+eul(3,1) = atan2(C(1,2),C(1,1));  % yaw
+
+% Ends

Calculate_errors_NED.m

@@ -0,0 +1,51 @@
+function [delta_r_eb_n,delta_v_eb_n,delta_eul_nb_n] = Calculate_errors_NED(...
+        est_L_b,est_lambda_b,est_h_b,est_v_eb_n,est_C_b_n,true_L_b,...
+        true_lambda_b,true_h_b,true_v_eb_n,true_C_b_n)
+%Calculate_errors_NED - Calculates the position, velocity, and attitude
+% errors of a NED navigation solution.
+%
+% Software for use with "Principles of GNSS, Inertial, and Multisensor
+% Integrated Navigation Systems," Second Edition.
+%
+% This function created 3/4/2012 by Paul Groves
+%
+% Inputs:
+%   est_L_b       latitude solution (rad)
+%   est_lambda_b  longitude solution (rad)
+%   est_h_b       height solution (m)
+%   est_v_eb_n    velocity solution of body frame w.r.t. ECEF frame,
+%                 resolved along north, east, and down (m/s)
+%   est_C_b_n     body-to-NED coordinate transformation matrix solution
+%   true_L_b      true latitude (rad)
+%   true_lambda_b true longitude (rad)
+%   true_h_b      true height (m)
+%   true_v_eb_n   true velocity of body frame w.r.t. ECEF frame, resolved
+%                 along north, east, and down (m/s)
+%   C_b_n         true body-to-NED coordinate transformation matrix
+%
+% Outputs:
+%   delta_r_eb_n     position error resolved along NED (m)
+%   delta_v_eb_n     velocity error resolved along NED (m/s)
+%   delta_eul_nb_n   attitude error as NED Euler angles (rad)
+%                    These are expressed about north, east, and down
+
+% Copyright 2012, Paul Groves
+% License: BSD; see license.txt for details
+
+% Begins
+
+% Position error calculation, using (2.119)
+[R_N,R_E] = Radii_of_curvature(true_L_b);
+delta_r_eb_n(1) = (est_L_b - true_L_b) * (R_N + true_h_b);
+delta_r_eb_n(2) = (est_lambda_b - true_lambda_b) * (R_E + true_h_b) *...
+    cos(true_L_b);
+delta_r_eb_n(3) = -(est_h_b - true_h_b);
+
+% Velocity error calculation
+delta_v_eb_n = est_v_eb_n - true_v_eb_n;
+
+% Attitude error calculation, using (5.109) and (5.111)
+delta_C_b_n = est_C_b_n * true_C_b_n';
+delta_eul_nb_n = -CTM_to_Euler(delta_C_b_n);
+
+% Ends

ECEF_to_ECI.m

@@ -0,0 +1,47 @@
+function [r_ib_i,v_ib_i,C_b_i] = ECEF_to_ECI(t,r_eb_e,v_eb_e,C_b_e)
+%ECEF_to_ECI - Converts position, velocity, and attitude from ECEF- to
+%ECI-frame referenced and resolved
+%地心地固坐标系转换到地心惯性坐标系
+% Software for use with "Principles of GNSS, Inertial, and Multisensor
+% Integrated Navigation Systems," Second Edition.
+%
+% This function created 2/4/2012 by Paul Groves
+%
+% Inputs:
+%   t             time (s)
+%   r_eb_e        Cartesian position of body frame w.r.t. ECEF frame, resolved
+%                 along ECEF-frame axes (m)
+%   v_eb_e        velocity of body frame w.r.t. ECEF frame, resolved along
+%                 ECEF-frame axes (m/s)
+%   C_b_e         body-to-ECEF-frame coordinate transformation matrix
+%
+% Outputs:
+%   r_ib_i        Cartesian position of body frame w.r.t. ECI frame, resolved
+%                 along ECI-frame axes (m)
+%   v_ib_i        velocity of body frame w.r.t. ECI frame, resolved along
+%                 ECI-frame axes (m/s)
+%   C_b_i         body-to-ECI-frame coordinate transformation matrix
+
+% Parameters
+omega_ie = 7.292115E-5;  % Earth rotation rate (rad/s)
+
+% Copyright 2012, Paul Groves
+% License: BSD; see license.txt for details
+
+% Begins
+
+% Calculate ECEF to ECI coordinate transformation matrix using (2.145)
+C_e_i = [cos(omega_ie * t), -sin(omega_ie * t), 0;...
+         sin(omega_ie * t),  cos(omega_ie * t), 0;...
+                         0,                  0, 1];
+
+% Transform position using (2.146)                   
+r_ib_i = C_e_i * r_eb_e;
+
+% Transform velocity using (2.145)
+v_ib_i = C_e_i * (v_eb_e + omega_ie * [-r_eb_e(2);r_eb_e(1);0]);
+
+% Transform attitude using (2.15)
+C_b_i = C_e_i * C_b_e;
+
+% Ends

ECEF_to_NED.m

@@ -0,0 +1,85 @@
+function [L_b,lambda_b,h_b,v_eb_n,C_b_n] = ECEF_to_NED(r_eb_e,v_eb_e,C_b_e)
+%ECEF_to_NED - Converts Cartesian  to curvilinear position, velocity
+%resolving axes from ECEF to NED and attitude from ECEF- to NED-referenced
+%
+% Software for use with "Principles of GNSS, Inertial, and Multisensor
+% Integrated Navigation Systems," Second Edition.
+%
+% This function created 2/4/2012 by Paul Groves
+%
+% Inputs:
+%   r_eb_e        Cartesian position of body frame w.r.t. ECEF frame, resolved
+%                 along ECEF-frame axes (m)
+%   v_eb_e        velocity of body frame w.r.t. ECEF frame, resolved along
+%                 ECEF-frame axes (m/s)
+%   C_b_e         body-to-ECEF-frame coordinate transformation matrix
+%
+% Outputs:
+%   L_b           latitude (rad)
+%   lambda_b      longitude (rad)
+%   h_b           height (m)
+%   v_eb_n        velocity of body frame w.r.t. ECEF frame, resolved along
+%                 north, east, and down (m/s)
+%   C_b_n         body-to-NED coordinate transformation matrix
+
+% Parameters
+R_0 = 6378137; %WGS84 Equatorial radius in meters
+e = 0.0818191908425; %WGS84 eccentricity
+
+% Copyright 2012, Paul Groves
+% License: BSD; see license.txt for details
+
+% Begins
+
+% Convert position using Borkowski closed-form exact solution
+% From (2.113)
+lambda_b = atan2(r_eb_e(2),r_eb_e(1));
+
+% From (C.29) and (C.30)
+k1 = sqrt(1 - e^2) * abs (r_eb_e(3));
+k2 = e^2 * R_0;
+beta = sqrt(r_eb_e(1)^2 + r_eb_e(2)^2);
+E = (k1 - k2) / beta;
+F = (k1 + k2) / beta;
+
+% From (C.31)
+P = 4/3 * (E*F + 1);
+
+% From (C.32)
+Q = 2 * (E^2 - F^2);
+
+% From (C.33)
+D = P^3 + Q^2;
+
+% From (C.34)
+V = (sqrt(D) - Q)^(1/3) - (sqrt(D) + Q)^(1/3);
+
+% From (C.35)
+G = 0.5 * (sqrt(E^2 + V) + E);
+
+% From (C.36)
+T = sqrt(G^2 + (F - V * G) / (2 * G - E)) - G;
+
+% From (C.37)
+L_b = sign(r_eb_e(3)) * atan((1 - T^2) / (2 * T * sqrt (1 - e^2)));
+
+% From (C.38)
+h_b = (beta - R_0 * T) * cos(L_b) +...
+    (r_eb_e(3) - sign(r_eb_e(3)) * R_0 * sqrt(1 - e^2)) * sin (L_b);
+
+% Calculate ECEF to NED coordinate transformation matrix using (2.150)
+cos_lat = cos(L_b);
+sin_lat = sin(L_b);
+cos_long = cos(lambda_b);
+sin_long = sin(lambda_b);
+C_e_n = [-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];
+
+% Transform velocity using (2.73)
+v_eb_n = C_e_n * v_eb_e;
+
+% Transform attitude using (2.15)
+C_b_n = C_e_n * C_b_e;
+
+% Ends