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.
|
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
- Copyright:
- Copyright (c) 2011 OpenShoe, ISC License (open source)
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_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 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] | P | Initial state covariance matrix. |
[out] | Q | Process noise covariance matrix. |
[out] | R | Measurement noise covariance matrix. |
[out] | H | Measurement observation 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] | 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 |