The OpenShoe Matlab Implementation
v1

This file contains all the functions needed to implement a zero velocity aided inertial navigation system. More...
Functions  
function  ZUPTaidedINS (in u, in zupt) 
Function that runs the zerovelocity aided INS Kalman filter algorithm.  
function  init_vec (in N, in P) 
Function that allocates memmory for the output of zerovelocity aided inertial navigation algorithm.  
function  init_Nav_eq (in u) 
Function that calculates the initial state of the navigation equations.  
function  init_filter () 
Function that initializes the Kalman filter.  
function  Navigation_equations (in x, in u, in q) 
The mechanized navigation equations of the inertial navigation system.  
function  state_matrix (in q, in u) 
Function for calculating the state transition matrix F and the process noise gain matrix G.  
function  comp_internal_states (in x_in, in dx, in q_in) 
Function that corrects the estimated navigation states with the by the Kalman filter estimated system perturbations (errors).  
function  comp_imu_errors (in u_in, in x_h) 
Function that compensats for the artifacts in the IMU measurements with the current estimates of the sensors biases and/or scale factor errors.  
function  q2dcm (in q) 
Function that converts a quaternion vector to a directional cosine matrix (rotation matrix)  
function  dcm2q (in R) 
Function that converts a directional cosine matrix (rotation matrix) in to a quaternion vector.  
function  Rt2b (in ang) 
Function that calculates the rotation matrix for rotating a vector from coordinate frame t to the coordinate frame b, given a vector of Euler angles. 
This file contains all the functions needed to implement a zero velocity aided inertial navigation system.
This file contains all the functions needed to implement a zerovelocity aided inertial navigation system, given a set of IMU data and a vector that indicates when the system has zerovelocity.
function comp_imu_errors  (  in  u_in, 
in  x_h  
) 
Function that compensats for the artifacts in the IMU measurements with the current estimates of the sensors biases and/or scale factor errors.
Function that compensats for the artifacts in the IMU measurements with the current estimates of the sensors biases and/or scale factor errors. If the sensor errors are not included in the state space model used in the Kalman filter, no correction/compensation is done.
[out]  u_out  Corrected/compensated IMU measurements. 
[in]  u_in  Raw IMU measurements. 
[in]  x_h  Navigation state vector, where the last states are the estimated sensor errors. 
function comp_internal_states  (  in  x_in, 
in  dx,  
in  q_in  
) 
Function that corrects the estimated navigation states with the by the Kalman filter estimated system perturbations (errors).
Function that corrects the estimated navigation states with the by the Kalman filter estimated system perturbations (errors). That is, the current position an velocity estimates of the navigation platform is corrected by adding the estimated system perturbations to these states. To correct the orientation state (Euler angles and quaternion vector), the quaternion vector are first converted into a rotation matrix, which then is corrected using the estimated orientation perturbations. The corrected rotation matrix is then transformed back into a quaternion vector, as well as the equivalent vector of Euler angles.
[out]  x_out  Corrected (posteriori) navigation state vector. 
[out]  q_out  Corrected (posteriori) quaternion vector. 
[in]  x_in  A priori estimated navigation state vector. 
[in]  q_in  A priori estimated quaternion vector. 
[in]  dx  Vector of system perturbations 
function dcm2q  (  in  R  ) 
Function that converts a directional cosine matrix (rotation matrix) in to a quaternion vector.
[out]  q  Quaternion vector. 
[in]  R  Rotation matrix. 
function init_filter  (  ) 
Function that initializes the Kalman filter.
Function that initializes the Kalman filter. That is, the function generates the initial covariance matrix P, the process noise covariance matrix Q, the measurement noise covariance matrix R, and observation matrix H, based upon the settings defined in the function settings.m
[out]  P  Initial state covariance matrix. 
[out]  Q  Process noise covariance matrix. 
[out]  R  Measurement noise covariance matrix. 
[out]  H  Measurement observation matrix. 
function init_Nav_eq  (  in  u  ) 
Function that calculates the initial state of the navigation equations.
Function that calculates the initial state of the navigation equations. That is, it does a simple initial alignment of the navigation system, where the roll and pitch of the system is estimated from the 20 first accelerometer readings. All other states are set according to the information given in the function "settings.m".
[out]  x  Initial navigation state vector. 
[out]  quat  Quaternion vector, representating the initial attitude of the platform. 
[in]  u  Matrix with the IMU data. 
function init_vec  (  in  N, 
in  P  
) 
Function that allocates memmory for the output of zerovelocity aided inertial navigation algorithm.
[out]  x_h  Matrix with the estimated navigation states. Each row holds the [position, velocity, attitude, (biases, if turned on),(scale factors, if turned on)] for time instant k, k=1,...N. 
[out]  cov  Matrix with the diagonal elements of the state covariance matrices. 
[out]  Id  Identity matrix. 
[in]  N  The length of the IMU data vector u, i.e., the number of samples. 
[in]  P  Initial state covariance matrix. 
function Navigation_equations  (  in  x, 
in  u,  
in  q  
) 
The mechanized navigation equations of the inertial navigation system.
The mechanized navigation equations of the inertial navigation system. That is, the function takes the old state (position, velocity, and attitude) of the navigation system, togheter with the current IMU data measurements (specific force, angular rates), and calculates the current state of the navigation system.
The mechanization of the navigation equations that has been implemented is very simple, and several higher order terms has been neglected. Therefore, this mechanization of the navigation equations should only be used in systems using lowcost sensor and where only moderate velocities can be expected.
[out]  y  New navigation state [position,velocity, attitude (euler angles]. 
[out]  q  New quaternions 
[in]  x  Old navigation state 
[in]  u  IMU data [specific force, angular rates]. 
[in]  q  Old quaternions 
function q2dcm  (  in  q  ) 
Function that converts a quaternion vector to a directional cosine matrix (rotation matrix)
[out]  R  Rotation matrix. 
[in]  q  Quaternion vector. 
function Rt2b  (  in  ang  ) 
Function that calculates the rotation matrix for rotating a vector from coordinate frame t to the coordinate frame b, given a vector of Euler angles.
[out]  R  Rotation matrix. 
[in]  ang  Euler angles [roll,pitch,heading] 
function state_matrix  (  in  q, 
in  u  
) 
Function for calculating the state transition matrix F and the process noise gain matrix G.
Function for calculating the state transition matrix F and the process noise gain matrix G, given the current orientation of the platform and the specific force vector.
[out]  F  State transition matrix. 
[out]  G  Process noise gain matrix. 
[in]  u  IMU data [specific force, angular rates]. 
[in]  q  Old quaternions 
function ZUPTaidedINS  (  in  u, 
in  zupt  
) 
Function that runs the zerovelocity aided INS Kalman filter algorithm.
Function that runs the zerovelocity aided INS Kalman filter algorithm. All settings for the filter is done in setting.m.
[out]  x_h  Matrix with the estimated navigation states. Each row holds the [position, velocity, attitude, (biases, if turned on),(scale factors, if turned on)] for time instant k, k=1,...N. 
[out]  cov  Matrix with the diagonal elements of the state covariance matrices. 
[in]  u  The IMU data vector. 
[in]  zupt  Vector with the decisions of the zerovelocity. 