The OpenShoe Matlab Implementation  v1
Functions
ZUPTaidedINS.m File Reference

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 zero-velocity aided INS Kalman filter algorithm.
function init_vec (in N, in P)
 Function that allocates memmory for the output of zero-velocity 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.

Detailed Description

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 zero-velocity aided inertial navigation system, given a set of IMU data and a vector that indicates when the system has zero-velocity.

Authors:
Isaac Skog, John-Olof Nilsson

Function Documentation

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.

Parameters:
[out]u_outCorrected/compensated IMU measurements.
[in]u_inRaw IMU measurements.
[in]x_hNavigation 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.

Parameters:
[out]x_outCorrected (posteriori) navigation state vector.
[out]q_outCorrected (posteriori) quaternion vector.
[in]x_inA priori estimated navigation state vector.
[in]q_inA priori estimated quaternion vector.
[in]dxVector of system perturbations
function dcm2q ( in  R)

Function that converts a directional cosine matrix (rotation matrix) in to a quaternion vector.

Parameters:
[out]qQuaternion vector.
[in]RRotation 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

Parameters:
[out]PInitial state covariance matrix.
[out]QProcess noise covariance matrix.
[out]RMeasurement noise covariance matrix.
[out]HMeasurement 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".

Parameters:
[out]xInitial navigation state vector.
[out]quatQuaternion vector, representating the initial attitude of the platform.
[in]uMatrix with the IMU data.
function init_vec ( in  N,
in  P 
)

Function that allocates memmory for the output of zero-velocity aided inertial navigation algorithm.

Parameters:
[out]x_hMatrix 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]covMatrix with the diagonal elements of the state covariance matrices.
[out]IdIdentity matrix.
[in]NThe length of the IMU data vector u, i.e., the number of samples.
[in]PInitial 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 low-cost sensor and where only moderate velocities can be expected.

Parameters:
[out]yNew navigation state [position,velocity, attitude (euler angles].
[out]qNew quaternions
[in]xOld navigation state
[in]uIMU data [specific force, angular rates].
[in]qOld quaternions
function q2dcm ( in  q)

Function that converts a quaternion vector to a directional cosine matrix (rotation matrix)

Parameters:
[out]RRotation matrix.
[in]qQuaternion 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.

Parameters:
[out]RRotation matrix.
[in]angEuler 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.

Parameters:
[out]FState transition matrix.
[out]GProcess noise gain matrix.
[in]uIMU data [specific force, angular rates].
[in]qOld quaternions
function ZUPTaidedINS ( in  u,
in  zupt 
)

Function that runs the zero-velocity aided INS Kalman filter algorithm.

Function that runs the zero-velocity aided INS Kalman filter algorithm. All settings for the filter is done in setting.m.

Parameters:
[out]x_hMatrix 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]covMatrix with the diagonal elements of the state covariance matrices.
[in]uThe IMU data vector.
[in]zuptVector with the decisions of the zero-velocity.
 All Files Functions