OpenShoe  1.0
Modules | Defines | Functions
OpenShoe filtering algorithms

Modules

 Auxilliary functions
 

Auxilliary functions for calculation various matrix and scalar operations.


 Miscellaneous
 

Miscellaneous routines.


 ZUPT aided INS functions
 

the main functions for running the ZUPT aided INS.


 Initialization routines
 

Routines for initializing the system. Only coarse initial alignment is implemented.


 Calibration routines
 

Calibration routines. Only accelerometer calibration is implemented since basic gyro calibration (bias) is trivial and is available on the IMU.


Defines

#define MAX_ORIENTATIONS   12
 Maximum numbers of orientations that can be used in the accelerometer calibration.
#define MIN_ORIENTATIONS   3
 Minimum numbers of orientations that can be used in the accelerometer calibration.
#define MATRIX_INVERSION_ERROR   1
 Value returned in the error message if an error occurred in the matrix inversion.
#define ACC_STATIONARITY_ERROR   2
 Value returned in the error message if the IMU was not stationary during the accelerometer calibration.
#define ACC_CALIBRATION_ILLCONDITIONED   3
 Value returned in the error message if the orientations used in the accelerometer calibration were poorly chosen.
#define NUMBER_OF_ORIENTATIONS_TO_LARGE   4
 Value returned in the error message if the number of orientations specified for the accelerometer calibration is to large. It has been changed to 12.
#define NUMBER_OF_ORIENTATIONS_TO_SMALL   5
 Value returned in the error message if the number of orientations specified for the accelerometer calibration is to few. It has been changed to 3.
#define absf(a)   (a>0 ? a:-a)
 Absolute value of a floating point variable.

Functions

void calibrate_accelerometers (void)
 Function for calibrating the accelerometer biases.
void estimate_accelerometer_biases (void)
 Function that estimates the accelerometer biases given a matrix of the mean of the measured acceleration at different orientations.
void correct_navigation_states (void)
 Function for correcting the navigation states given a zero-velocity detection.
void gain_matrix (void)
 Function for calculating the Kalman filter gain matrix.
void initialize_navigation_algorithm (void)
 Function for initializing the navigation algorithm.
void measurement_update (void)
 Function for doing a measurement update of the Kalman filter covariance.
void strapdown_mechanisation_equations (void)
 Function for doing a time update of the mechanized navigation equations.
void time_up_data (void)
 Function for doing a time update of the Kalman filter state covariance.
void update_imu_data_buffers (void)
 Function that updates the IMU data buffers with the latest values read from the IMU, and writes the IMU data to that should be process at the current iteration to the processing variables.
void ZUPT_detector (void)
 Function for detecting when the system has zero-velocity.
void zupt_update (void)
 Wrapper function that checks if a zero-velocity update should be done, and then calls all navigation algorithm functions that should be executed during a zero-velocity update.

Accelerometer calibration parameters.

Parameters controlling accelerometer calibration, and vectors and matrices used store the biases.

vec3 accelerometer_biases
 Accelerometer biases (x,y,z-axis) [ $m/s^2$].
precision acceleration_variance_threshold = 0.002
 Threshold used to check that accelerometers were stationary during the calibration [ $(m/s^2)^2$].
uint32_t nr_of_calibration_samples = 800
 Number of samples used at each orientation in the calibration procedure.
uint8_t nr_of_calibration_orientations = 6
 Number of orientations used in the accelerometer calibration procedure. OBS! Most be at least 3 and less than 13.
Bool new_orientation_flag = false
 Flag that is set to true when the IMU should be place in a new orientation. Should be set to false when the calibration procedure is started, and when the IMU has been placed in a new orientation by the user.
Bool acc_calibration_finished_flag = false
 Flag that is set to true when the calibration is finished. Must be set to false before the calibration is started.

General control parameters.

Parameters controlling general settings of the navigation algorithm

precision latitude = 13
 Rough latitude of the system [ $degrees$]. (Used to calculate the magnitude of the gravity vector)
precision altitude = 920
 Rough altitude of the system [ $m$]. (Used to calculate the magnitude of the gravity vector)
precision g = 9.782940329221166
 Magnitude of the local gravity acceleration [ $m/s^2$].
precision dt = 0.001220703125000
 Sampling period [ $s$].
uint8_t error_signal
 Error signaling vector. If zero no error has occurred.

IMU data buffer variables.

Vectors and variables related to the IMU data buffer.

vec3 accelerations_in
 Accelerations read from the IMU [ $m/s^2$]. These are written into the IMU data buffer.
vec3 angular_rates_in
 Angular rates read from the IMU [ $rad/s$]. These are written into the IMU data buffer.
vec3 accelerations_out
 Accelerations outputted from the IMU data buffer [ $m/s^2$].
vec3 angular_rates_out
 Angular rates outputted from the IMU data buffer [ $rad/s$].

Initialization control parameters

Parameters controlling the initialization of the navigation algorithm, i.e., the initial states of the inertial navigation system equations and the initial Kalman filter covariance matrix.

Bool initialize_flag = true
 A flag that should be set to true when initialization is started and that becomes false when the initialization is finished.
uint8_t nr_of_inital_alignment_samples = 16
 Number of samples used in the initial alignment.
precision initial_heading = 0
 Initial heading [ $rad$].
vec3 initial_pos = {0, 0, 0}
 Initial position (North, East, Down) [ $m$].
vec3 sigma_initial_position = {0.00001,0.00001,0.00001}
 Standard deviations in the initial position uncertainties [ $m$].
vec3 sigma_initial_velocity = {0.01,0.01,0.01}
 Standard deviations in the initial velocity uncertainties [ $m/s$].
vec3 sigma_initial_attitude = {0.00174,0.00174,0.00174}
 Standard deviations in the initial attitude uncertainties [ $rad$].

Kalman filter control parameters

Parameters controlling the behavior of the Kalman filter. The parameters can be changed while the filter is running to adapt the filter to the current motion dynamics.

Note:
The default noise standard deviation figures are not set to reflect the true noise figures of the IMU sensors, but rather to model the sum of all the errors (biases, scale factors, nonlinearities, etc.) in the system and the measurement model.
precision sigma_acceleration = 0.7
 Accelerometer process noise standard deviation [ $m/s^2$].
precision sigma_gyroscope = 0.005235987755983
 Gyroscope process noise standard deviation [ $rad/s$].
vec3 sigma_velocity = {0.1,0.1,0.1}
 Pseudo zero-velocity measurement noise standard deviations (north, east, down) [ $m/s$].

Navigation and filter state variables.

Vectors that holds the current navigation state estimate and the covariance and gain of the Kalman filter.

vec3 position
 Position estimate (North,East,Down) [ $m$].
vec3 velocity
 Velocity estimate (North,East,Down) [ $m/s$].
quat_vec quaternions
 Attitude (quaternions) estimate.
mat3 Rb2t
 Rotation matrix used as an "aiding" variable in the filter algorithm. Holds the same information as the quaternions.
mat9sym cov_vector
 Vector representation of the Kalman filter covariance matrix.
mat9by3 kalman_gain
 Vector representation of the Kalman filter gain matrix.

Zero-velocity detector control parameters

Parameters controlling the behavior of the zero-velocity detector. All the detector control parameters, except the detector_Window_size may be changed will the navigation algorithm is running in order to adapt the behavior of the detector to the current motion dynamics.

precision sigma_acc_det = 0.035
 Accelerometer noise standard deviation figure [ $m/s^2$], which is used to control how much the detector should trusts the accelerometer data.
precision sigma_gyro_det = 0.006
 Gyroscope noise standard deviation figure [ $rad/s$], which is used to control how much the detector should trusts the gyroscope data.
volatile uint8_t detector_Window_size = 3
 The data window size used in the detector (OBS! Must be an odd number.).
precision detector_threshold = 50000
 Threshold used in the detector.
bool zupt = false
 Flag that is set to true if a zero-velocity update should be done.
precision Test_statistics = 0
 Variable holding the test statistics for the generalized likelihood ratio test, i.e., the zero-velocity detector.

Detailed Description

This group contain the filtering algorithms written for OpenShoe. That is, the ZUPT aided INS and calibration algorithms

Define Documentation

#define absf (   a)    (a>0 ? a:-a)

Absolute value of a floating point variable.

#define ACC_CALIBRATION_ILLCONDITIONED   3

Value returned in the error message if the orientations used in the accelerometer calibration were poorly chosen.

#define ACC_STATIONARITY_ERROR   2

Value returned in the error message if the IMU was not stationary during the accelerometer calibration.

#define MATRIX_INVERSION_ERROR   1

Value returned in the error message if an error occurred in the matrix inversion.

#define MAX_ORIENTATIONS   12

Maximum numbers of orientations that can be used in the accelerometer calibration.

#define MIN_ORIENTATIONS   3

Minimum numbers of orientations that can be used in the accelerometer calibration.

#define NUMBER_OF_ORIENTATIONS_TO_LARGE   4

Value returned in the error message if the number of orientations specified for the accelerometer calibration is to large. It has been changed to 12.

#define NUMBER_OF_ORIENTATIONS_TO_SMALL   5

Value returned in the error message if the number of orientations specified for the accelerometer calibration is to few. It has been changed to 3.

Function Documentation

void calibrate_accelerometers ( void  )

Function for calibrating the accelerometer biases.

This function is used to calibrate the biases of the accelerometers in the IMU and requires the users to place the IMU into at least three different orientations. The calibration method is based upon the calibration algorithm described in the paper Calibration of the Accelerometer Triad of an Inertial Measurement Unit, Maximum Likelihood Estimation and Cramer-Rao Bound, but does only calibrate the accelerometer bias.

Before the calibration is started the flags new_orientation_flag and acc_calibration_successful_flag should be set to false. Then the function should called every time new IMU-data have been read from the IMU and as long as the new flags new_orientation_flag and acc_calibration_successful_flag are false. When the flag new_orientation_flag becomes true a message should be sent to the user, which then should place the IMU in a new orientation and reset the flag. When the flag acc_calibration_successful_flag becomes true the calibration is finished and the accelerometer calibration parameters have been written into the memory of the IMU.

Note:
The function can return one error and one warning message that are stored in the variable #error_vec. The error message may be sent if the was no stationary during one of the calibration phases and new accelerometer data most be recorded with the IMU in the same orientation. A warning message may be sent if the orientations the IMU was placed in may have caused a poor estimate of the accelerometer biases.
Parameters:
[out]error_vecA variable that is set non zero value of an error/warning has occurred during the
[in,out]acc_calibration_successful_flagA flag that should be false when calibration is started and that becomes true when the calibration is finished.
[in,out]new_orientation_flagA flag that should be false when calibration is started and that becomes true when IMU should be placed in a new orientation. When the IMU has been placed in a new orientation it should be set to false. execution of the function.
[in]accelerations_inThe from the IMU latest read acceleration data.
void correct_navigation_states ( void  )

Function for correcting the navigation states given a zero-velocity detection.

When called this function takes the velocity estimate of the navigation system and treat it as an observation of the current velocity error of the system. The errors in all navigation states (position, velocity, and attitude) are then estimated by multiplying the "velocity error" with the Kalman gain. The estimated errors in the navigation states are then used to correct the navigation states.

Parameters:
[in,out]positionThe position estimate of the navigation system.
[in,out]velocityThe velocity estimate of the navigation system.
[out]quaternionsThe orientation estimate of the navigation system.
[in]Rb2tThe vector representation of the body to navigation coordinate system rotation matrix estimate.
[in]kalman_gainThe vector representation of the Kalman filter gain matrix.
void estimate_accelerometer_biases ( void  )

Function that estimates the accelerometer biases given a matrix of the mean of the measured acceleration at different orientations.

Parameters:
[out]max_vLargest value of the input vector.
[out]indexIndex of the vector element holding the largest value.
[in]arg_vecThe input vector.
void gain_matrix ( void  )

Function for calculating the Kalman filter gain matrix.

When called the function calculates the Kalman filter gain and store it in the vector kalman_gain. To calculate the gain the function uses the Kalman filter covariance and the pseudo velocity measurement noise standard deviation.

Parameters:
[out]kalman_gainThe vector representation of the Kalman filter gain matrix.
[in]cov_vectorThe vector representation of the Kalman filter covariance matrix.
[in]sigma_velocityThe standard deviation of the pseudo velocity measurement error (Note: This parameters enters the function through a sub-function.).
void initialize_navigation_algorithm ( void  )

Function for initializing the navigation algorithm.

This function initializes the navigation algorithm and should during the initialization of the navigation system be called every time new IMU-data have been read from the IMU. Before the initialization is started the flag initialize_flag should be set to true, and the counter initialize_sample_ctr to zero. The initialization is finished when the flag initialize_flag becomes false.

The initialization function first runs an initial alignment of the navigation system, where the roll and pitch are estimated from the average of the accelerometer readings. Then, the function sets the initial navigation states (position, velocity, and quaternions) and the initial Kalman filter covariance.

Note:
The navigation system most be stationary during the initialization, and the number of samples used in the initial alignment most be larger than the length of the zero-velocity detector window.
Parameters:
[out]positionThe position estimate of the navigation system.
[out]velocityThe velocity estimate of the navigation system.
[out]quaternionsThe orientation estimate of the navigation system.
[out]cov_vectorThe vector representation of the Kalman filter covariance matrix.
[in,out]initialize_flagA flag that should be set to true when initialization is started and that becomes false when the initialization is finished.
[in]nr_of_inital_alignment_samplesThe number of samples used in the initial alignment.
[in]initial_headingThe initial heading of the navigation system.
[in]initial_posThe initial position of the navigation system.
[in]sigma_initial_positionThe standard deviations of the uncertainties in the initial position.
[in]sigma_initial_velocityThe standard deviations of the uncertainties in the initial velocity.
[in]sigma_initial_attitudeThe standard deviations of the uncertainties in the initial attitude.
void measurement_update ( void  )

Function for doing a measurement update of the Kalman filter covariance.

When called the function does a measurement update of the Kalman filter state covariance matrix stored in the vector cov_vector.

Parameters:
[in,out]cov_vectorThe vector representation of the Kalman filter covariance matrix.
[in]kalman_gainThe vector representation of the Kalman filter gain matrix.
void strapdown_mechanisation_equations ( void  )

Function for doing a time update of the mechanized navigation equations.

When called the function does a time update of the mechanized inertial navigation system equations. That is, first the quaternions stored in quaternions is updated using the angular rate measurements in angular_rates_out. Then the position and velocity state vectors position and velocity are updated using the acceleration measurements in accelerations_out. The function also updates the ''aiding'' vector Rb2t (body to navigation coordinate system rotation matrix) which is used in the Kalman filter.

Note:
This is a rudimentary mechanization of the inertial navigation equations which only is suitable for use with systems that uses low-cost/low-performance inertial sensors and where only short periods of free inertial navigation is expected.
Parameters:
[in,out]positionThe position estimate of the navigation system.
[in,out]velocityThe velocity estimate of the navigation system.
[in,out]quaternionsThe orientation estimate of the navigation system.
[out]Rb2tThe body to navigation coordinate system rotation matrix estimate.
[in]accelerations_outThe acceleration measurements used in the update of the inertial navigation system equations.
[in]angular_rates_outThe angular rate measurements used in the update of the inertial navigation system equations.
[in]dtThe sampling period of the system.
[in]gThe magnitude of the local gravity vector.
void time_up_data ( void  )

Function for doing a time update of the Kalman filter state covariance.

When called the function does a time update of the Kalman filter state covariance matrix stored in the vector cov_vector.

Parameters:
[in,out]cov_vectorThe vector representation of the Kalman filter covariance matrix.
[in]dtThe sampling period of the system.
[in]sigma_accelerationThe standard deviation of the accelerometer process noise.
[in]sigma_gyroscopeThe standard deviation of the gyroscope process noise.
[in]accelerations_outThe acceleration measurements used in the update of the inertial navigation system equations.
[in]Rb2tThe vector current body to navigation coordinate system rotation matrix estimate.
void update_imu_data_buffers ( void  )

Function that updates the IMU data buffers with the latest values read from the IMU, and writes the IMU data to that should be process at the current iteration to the processing variables.

The function updates the IMU data buffers acc_buffer_x_axis, acc_buffer_y_axis, acc_buffer_z_axis, gyro_buffer_x_axis, gyro_buffer_y_axis, gyro_buffer_z_axis with the values stored in the vectors accelerations_in and angular_rates_in. The function also updates the vectors accelerations_out and angular_rates_out. The data stored in these vectors is the data processed in the next iteration of the navigation algorithm.

Note:
This function should be called after data have been read from the IMU through the SPI interface and before the navigation algorithm is processed.
Parameters:
[out]accelerations_outThe acceleration data that is to be used in the next iteration of the navigation algorithm.
[out]angular_rates_outThe angular rate data that is to be used in the next iteration of the navigation algorithm.
[in]accelerations_inThe from the IMU latest read acceleration data.
[in]angular_rates_inThe from the IMU latest read angular rate data.
void ZUPT_detector ( void  )

Function for detecting when the system has zero-velocity.

When called this function takes the acceleration and angular rate measurements stored in the IMU data buffers and runs a generalized likelihood ratio test to determine if the navigation system is stationary or moving. More information about the detector and it characteristics can be found in following papers:

Parameters:
[out]zuptThe zero-velocity detection flag
[in]detector_Window_sizeThe window size of the zero-velocity detector.
[in]detector_thresholdThe threshold used in the detector.
[in]sigma_acc_detThe standard deviation figure used to control the importance of the accelerometer measurements in the detection algorithm.
[in]sigma_gyro_detThe standard deviation figure used to control the importance of the gyroscope measurements in the detection algorithm.
[in]gThe magnitude of the local gravity vector.
void zupt_update ( void  )

Wrapper function that checks if a zero-velocity update should be done, and then calls all navigation algorithm functions that should be executed during a zero-velocity update.

Wrapper function that checks if a zero-velocity update should be done, and then calls all navigation algorithm functions that should be executed during a zero-velocity update. The function first calls ZUPT_detector. If then flag zupt is set to true, it also calls the following functions:

  • gain_matrix
  • correct_navigation_states
  • measurement_update

Variable Documentation

Bool acc_calibration_finished_flag = false

Flag that is set to true when the calibration is finished. Must be set to false before the calibration is started.

precision acceleration_variance_threshold = 0.002

Threshold used to check that accelerometers were stationary during the calibration [ $(m/s^2)^2$].

vec3 accelerations_in

Accelerations read from the IMU [ $m/s^2$]. These are written into the IMU data buffer.

vec3 accelerations_out

Accelerations outputted from the IMU data buffer [ $m/s^2$].

vec3 accelerometer_biases

Accelerometer biases (x,y,z-axis) [ $m/s^2$].

precision altitude = 920

Rough altitude of the system [ $m$]. (Used to calculate the magnitude of the gravity vector)

vec3 angular_rates_in

Angular rates read from the IMU [ $rad/s$]. These are written into the IMU data buffer.

vec3 angular_rates_out

Angular rates outputted from the IMU data buffer [ $rad/s$].

mat9sym cov_vector

Vector representation of the Kalman filter covariance matrix.

precision detector_threshold = 50000

Threshold used in the detector.

volatile uint8_t detector_Window_size = 3

The data window size used in the detector (OBS! Must be an odd number.).

precision dt = 0.001220703125000

Sampling period [ $s$].

uint8_t error_signal

Error signaling vector. If zero no error has occurred.

precision g = 9.782940329221166

Magnitude of the local gravity acceleration [ $m/s^2$].

precision initial_heading = 0

Initial heading [ $rad$].

vec3 initial_pos = {0, 0, 0}

Initial position (North, East, Down) [ $m$].

Bool initialize_flag = true

A flag that should be set to true when initialization is started and that becomes false when the initialization is finished.

mat9by3 kalman_gain

Vector representation of the Kalman filter gain matrix.

precision latitude = 13

Rough latitude of the system [ $degrees$]. (Used to calculate the magnitude of the gravity vector)

Bool new_orientation_flag = false

Flag that is set to true when the IMU should be place in a new orientation. Should be set to false when the calibration procedure is started, and when the IMU has been placed in a new orientation by the user.

uint8_t nr_of_calibration_orientations = 6

Number of orientations used in the accelerometer calibration procedure. OBS! Most be at least 3 and less than 13.

uint32_t nr_of_calibration_samples = 800

Number of samples used at each orientation in the calibration procedure.

uint8_t nr_of_inital_alignment_samples = 16

Number of samples used in the initial alignment.

vec3 position

Position estimate (North,East,Down) [ $m$].

quat_vec quaternions

Attitude (quaternions) estimate.

mat3 Rb2t

Rotation matrix used as an "aiding" variable in the filter algorithm. Holds the same information as the quaternions.

precision sigma_acc_det = 0.035

Accelerometer noise standard deviation figure [ $m/s^2$], which is used to control how much the detector should trusts the accelerometer data.

precision sigma_acceleration = 0.7

Accelerometer process noise standard deviation [ $m/s^2$].

precision sigma_gyro_det = 0.006

Gyroscope noise standard deviation figure [ $rad/s$], which is used to control how much the detector should trusts the gyroscope data.

precision sigma_gyroscope = 0.005235987755983

Gyroscope process noise standard deviation [ $rad/s$].

vec3 sigma_initial_attitude = {0.00174,0.00174,0.00174}

Standard deviations in the initial attitude uncertainties [ $rad$].

vec3 sigma_initial_position = {0.00001,0.00001,0.00001}

Standard deviations in the initial position uncertainties [ $m$].

vec3 sigma_initial_velocity = {0.01,0.01,0.01}

Standard deviations in the initial velocity uncertainties [ $m/s$].

vec3 sigma_velocity = {0.1,0.1,0.1}

Pseudo zero-velocity measurement noise standard deviations (north, east, down) [ $m/s$].

precision Test_statistics = 0

Variable holding the test statistics for the generalized likelihood ratio test, i.e., the zero-velocity detector.

vec3 velocity

Velocity estimate (North,East,Down) [ $m/s$].

bool zupt = false

Flag that is set to true if a zero-velocity update should be done.

 All Data Structures Files Functions Variables Typedefs Defines
Generated on Mon Dec 19 2011 21:29:29 for OpenShoe by   doxygen 1.7.5.1