Initialization routines

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

Functions
void	initialize_navigation_algorithm (void)
	Function for initializing the navigation algorithm.

---

Detailed Description

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

---

Function Documentation

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]	position	The position estimate of the navigation system.
[out]	velocity	The velocity estimate of the navigation system.
[out]	quaternions	The orientation estimate of the navigation system.
[out]	cov_vector	The vector representation of the Kalman filter covariance matrix.
[in,out]	initialize_flag	A 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_samples	The number of samples used in the initial alignment.
[in]	initial_heading	The initial heading of the navigation system.
[in]	initial_pos	The initial position of the navigation system.
[in]	sigma_initial_position	The standard deviations of the uncertainties in the initial position.
[in]	sigma_initial_velocity	The standard deviations of the uncertainties in the initial velocity.
[in]	sigma_initial_attitude	The standard deviations of the uncertainties in the initial attitude.