OpenShoe  1.0
Functions
nav_eq.c File Reference

The c-file for the OpenShoe navigation algorithm. More...

#include "nav_eq.h"

Functions

precision sqrt_hf (precision arg)
 Function for calculating the square root.
precision vecnorm2 (precision *arg_vec, uint8_t len)
 Function that calculates the squared Euclidean norm of a vector.
void euler2rotation (mat3 rotmat, const vec3 euler)
 Function that converts Euler angles ([roll,pitch,yaw]) into a rotation matrix $R_b^t$.
void rotation2quat (quat_vec q, const mat3 rotmat)
 Function for converting a rotation matrix $R_b^t$ to quaternions.
void quat2rotation (mat3 rotmat, const quat_vec q)
 Function for converting quaternions to a rotation matrix $R_b^t$.
void rotation2euler (vec3 euler, const mat3 rotmat)
 Function for converting a rotation matrix $R_b^t$ to Euler angles ([roll,pitch,yaw]).
void innovation_cov (mat3sym re, mat9sym pvec)
 Function for calculating the Kalman filter innovation covariance.
void invmat3sys (mat3sym ainv, mat3sym a)
 Function for inverting a 3 by 3 matrix hermitian matrix.
void max_value (precision *max_v, uint8_t *index, precision *arg_vec)
 Function that calculates the maximum value of a vector and returns the max value and the index of the vector element holding the maximum value.
void gravity (void)
 Function that calculates the magnitude of the local gravity vector based upon the WGS84 gravity model.
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 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 gain_matrix (void)
 Function for calculating the Kalman filter gain matrix.
void measurement_update (void)
 Function for doing a measurement update of the Kalman filter covariance.
void correct_navigation_states (void)
 Function for correcting the navigation states given a zero-velocity detection.
void ZUPT_detector (void)
 Function for detecting when the system has zero-velocity.
void initialize_navigation_algorithm (void)
 Function for initializing the navigation algorithm.
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 calibrate_accelerometers (void)
 Function for calibrating the accelerometer biases.
void zupt_update (void)
 Routine collecting the functions which need to be run to make a ZUPT update.

Variables

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

The c-file for the OpenShoe navigation algorithm.

This is the c-file for the OpenShoe navigation algorithm. It includes the signal processing functions needed to implement a zero-velocity aided inertial navigation system using a nine state Kalman filter working in a complimentary feedback configuration. It also includes the functions needed for implementing a accelerometer bias calibration framework.

Authors:
John-Olof Nilsson, Isaac Skog
Copyright:
Copyright (c) 2011 OpenShoe, ISC License (open source)
 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