INS_kf Class Reference

Altitude estimator. More...

#include <ins_kf.hpp>

Inheritance diagram for INS_kf:

Collaboration diagram for INS_kf:

List of all members.

Classes
struct	conf_t
	Configuration structure. More...
Public Member Functions
	INS_kf (State &state, const Gps &gps, const Gps_mocap &gps_mocap, const Barometer &barometer, const Sonar &sonar, const PX4Flow &flow, const AHRS &ahrs, const conf_t config=default_config())
	Constructor.
bool	update (void)
	Main update function.
float	last_update_s (void) const
	Last update in seconds.
std::array< float, 3 >	position_lf (void) const
	3D Position in meters (NED frame)
std::array< float, 3 >	velocity_lf (void) const
	Velocity in meters/seconds in NED frame.
float	absolute_altitude (void) const
	Absolute altitude above sea level in meters (>=0)
bool	is_healthy (INS::healthy_t type) const
	Indicates which estimate can be trusted.
void	init (void)
	Initialize/reset the kalman filter.
Static Public Member Functions
static INS_kf::conf_t	default_config (void)
	Default configuration structure.
Public Attributes
conf_t	config_
	Configuration (public, to be used as onboard param)
uint32_t	init_flag
	Flag used to force initialization by telemetry (0 no init, otherwise init)
Friends
void	ins_telemetry_send (const INS_kf *ins, const Mavlink_stream *mavlink_stream, mavlink_message_t *msg)

---

Detailed Description

Altitude estimator.

• state vector X (n=11): X = [ x_ned, y_ned, z_ned, z_ground_ned, vx, vy, vz, bias_accx, bias_accy, bias_accz bias_baro ]

• model A = [ 1, 0, 0, 0, dt, 0, 0, -(1/2)*alpha_x*(dt^2), -(1/2)*beta_x*(dt^2), -(1/2)*gamma_x*(dt^2), 0 ] // Pos x integrated with vel and inclue acc biases in global ref [ 0, 1, 0, 0, 0, dt, 0, -(1/2)*alpha_y*(dt^2), -(1/2)*beta_y*(dt^2), -(1/2)*gamma_y*(dt^2), 0 ] // Pos y integrated with vel and inclue acc biases in global ref [ 0, 0, 1, 0, 0, 0, dt, -(1/2)*alpha_z*(dt^2), -(1/2)*beta_z*(dt^2), -(1/2)*gamma_z*(dt^2), 0 ] // Pos z integrated with vel and inclue acc biases in global ref [ 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0 ] // Ground alt (cst) [ 0, 0, 0, 0, 1, 0, 0, -alpha_x*dt, -beta_x*dt, -gamma_x*dt, 0 ] // Vel x include acc bias in global ref [ 0, 0, 0, 0, 0, 1, 0, -alpha_y*dt, -beta_y*dt, -gamma_y*dt, 0 ] // Vel y include acc bias in global ref [ 0, 0, 0, 0, 0, 0, 1, -alpha_z*dt, -beta_z*dt, -gamma_z*dt, 0 ] // Vel z include acc bias in global ref [ 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0 ] // Acc x bias (cst) [ 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0 ] // Acc y bias (cst) [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0 ] // Acc z bias (cst) [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 ] // baro bias (cst) with: alpha_x = q0^2 + q1^2 - q2^2 - q3^2 (using attitude quaternion q = (q0, q1, q2, q3)') beta_x = 2*(-q0*q3 + q1*q2) gamma_x = 2*(q0*q2 + q1*q3) alpha_y = 2*(q0*q3 + q1*q2) beta_y = q0^2 - q1^2 + q2^2 - q3^2 gamma_y = 2*(-q0*q1 + q2*q3) alpha_z = 2*(-q0*q2 + q1*q3) beta_z = 2*(q0*q1 + q2*q3) gamma_z = q0^2 - q1^2 - q2^2 - q3^2

• input u (p=3): u = [ ax, ay, az ] // Accelerations in body frame

• input model B = [ (1/2)*alpha_x*(dt^2), (1/2)*beta_x*(dt^2), (1/2)*gamma_x*(dt^2) ] [ (1/2)*alpha_y*(dt^2), (1/2)*beta_y*(dt^2), (1/2)*gamma_y*(dt^2) ] [ (1/2)*alpha_z*(dt^2), (1/2)*beta_z*(dt^2), (1/2)*gamma_z*(dt^2) ] [ 0, 0, 0 ] [ alpha_x*dt, beta_x*dt, gamma_x*dt ] [ alpha_y*dt, beta_y*dt, gamma_y*dt ] [ alpha_z*dt, beta_z*dt, gamma_z*dt ] [ 0, 0, 0 ] [ 0, 0, 0 ] [ 0, 0, 0 ] [ 0, 0, 0 ]

• measurement 1 (gps) : z1 = [ x_ned, y_ned, z_ned ] H1 = [ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ] [ 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 ] [ 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0 ]

• measurement 2 (gps) : z2 = [ vx, vy, vz ] H2 = [ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 ] [ 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 ] [ 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0 ]

• measurement 3 (baro): z3 = [ -z_ned - bias_baro ] H3 = [ 0, 0, -1, 0, 0, 0, 0, 0, 0, 0, -1 ]

• measurement 4 (sonar): z4 = [ -z_ned + z_ground_ned ] H4 = [ 0, 0, -1, 1, 0, 0, 0, 0, 0, 0, 0 ]

---

Member Function Documentation

float INS_kf::absolute_altitude

(

void

)

const [virtual]

Absolute altitude above sea level in meters (>=0)

Returns:

altitude

Implements INS.

Here is the call graph for this function:

bool INS_kf::is_healthy

(

INS::healthy_t

type

)

const [virtual]

Indicates which estimate can be trusted.

Parameters:

type	Type of estimate

Returns:

boolean

Implements INS.

Here is the call graph for this function:

float INS_kf::last_update_s

(

void

)

const [virtual]

Last update in seconds.

Returns:

time

Implements INS.

std::array< float, 3 > INS_kf::position_lf

(

void

)

const [virtual]

3D Position in meters (NED frame)

Returns:

position

Implements INS.

bool INS_kf::update

(

void

)

[virtual]

Main update function.

Returns:

Success

Implements INS.

Here is the call graph for this function:

std::array< float, 3 > INS_kf::velocity_lf

(

void

)

const [virtual]

Velocity in meters/seconds in NED frame.

Returns:

velocity

Implements INS.

---

The documentation for this class was generated from the following files:

• /home/travis/build/lis-epfl/MAVRIC_Library/sensing/ins_kf.hpp
• /home/travis/build/lis-epfl/MAVRIC_Library/sensing/ins_kf.cpp