Paparazzi UAS  v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ekf_range.c File Reference
#include "ekf_range.h"
#include <math.h>
+ Include dependency graph for ekf_range.c:

Go to the source code of this file.

Functions

void ekf_range_init (struct EKFRange *ekf_range, float P0_pos, float P0_speed, float Q_sigma2, float R_dist, float R_speed, float dt)
 Init EKF_range internal struct. More...
 
void ekf_range_set_state (struct EKFRange *ekf_range, struct EnuCoor_f pos, struct EnuCoor_f speed)
 Set initial state vector. More...
 
void ekf_range_get_state (struct EKFRange *ekf_range, struct EnuCoor_f *pos, struct EnuCoor_f *speed)
 Get current state. More...
 
struct EnuCoor_f ekf_range_get_pos (struct EKFRange *ekf_range)
 Get current pos. More...
 
struct EnuCoor_f ekf_range_get_speed (struct EKFRange *ekf_range)
 Get current speed. More...
 
void ekf_range_update_noise (struct EKFRange *ekf_range, float Q_sigma2, float R_dist, float R_speed)
 Update process and measurement noises. More...
 
void ekf_range_predict (struct EKFRange *ekf_range)
 propagate dynamic model More...
 
void ekf_range_update_dist (struct EKFRange *ekf_range, float dist, struct EnuCoor_f anchor)
 correction step More...
 
void ekf_range_update_speed (struct EKFRange *ekf_range, float speed, uint8_t type)
 Update step based on speed measure. More...
 

Detailed Description

Author
Gautier Hattenberger EKF based on range measurements algorithm

Definition in file ekf_range.c.

Function Documentation

struct EnuCoor_f ekf_range_get_pos ( struct EKFRange ekf_range)

Get current pos.

Parameters
[in]ekf_rangeEKFRange structure
Returns
current position

Definition at line 75 of file ekf_range.c.

References EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.

Referenced by check_and_compute_data(), and dw1000_arduino_periodic().

+ Here is the caller graph for this function:

struct EnuCoor_f ekf_range_get_speed ( struct EKFRange ekf_range)

Get current speed.

Parameters
[in]ekf_rangeEKFRange structure
Returns
current speed

Definition at line 84 of file ekf_range.c.

References EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.

Referenced by check_and_compute_data(), and dw1000_arduino_periodic().

+ Here is the caller graph for this function:

void ekf_range_get_state ( struct EKFRange ekf_range,
struct EnuCoor_f pos,
struct EnuCoor_f speed 
)

Get current state.

Parameters
[in]ekf_rangeEKFRange structure
[out]poscurrent position
[out]speedcurrent speed

Definition at line 65 of file ekf_range.c.

References EKFRange::state, EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.

void ekf_range_init ( struct EKFRange ekf_range,
float  P0_pos,
float  P0_speed,
float  Q_sigma2,
float  R_dist,
float  R_speed,
float  dt 
)

Init EKF_range internal struct.

Parameters
[in]ekf_rangeEKFRange structure
[in]P0_posinitial covariance on position
[in]P0_speedinitial covariance on speed
[in]Q_sigma2process noise
[in]R_distmeasurement noise on distance
[in]R_speedmeasurement noise on speed
[in]dtprediction time step in seconds

Definition at line 29 of file ekf_range.c.

References EKFRange::dt, EKF_RANGE_DIM, EKFRange::P, EKFRange::Q, EKFRange::R_dist, EKFRange::R_speed, and EKFRange::state.

Referenced by dw1000_arduino_init().

+ Here is the caller graph for this function:

void ekf_range_predict ( struct EKFRange ekf_range)

propagate dynamic model

Prediction step.

F = [ 1 dt 0 0 0 0 0 1 0 0 0 0 0 0 1 dt 0 0 0 0 0 1 0 0 0 0 0 0 1 dt 0 0 0 0 0 1 ]

Definition at line 119 of file ekf_range.c.

References EKFRange::dt, EKF_RANGE_DIM, EKFRange::P, EKFRange::Q, and EKFRange::state.

Referenced by dw1000_arduino_periodic().

+ Here is the caller graph for this function:

void ekf_range_set_state ( struct EKFRange ekf_range,
struct EnuCoor_f  pos,
struct EnuCoor_f  speed 
)

Set initial state vector.

This function should be called after initialization of the ekf struct and before running the filter for better results and faster convergence

Parameters
[in]ekf_rangeEKFRange structure
[in]posinitial position
[in]speedinitial speed

Definition at line 55 of file ekf_range.c.

References EKFRange::state, EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.

Referenced by check_and_compute_data().

+ Here is the caller graph for this function:

void ekf_range_update_dist ( struct EKFRange ekf_range,
float  dist,
struct EnuCoor_f  anchor 
)

correction step

Update step based on each new distance data.

K = PHt(HPHt+R)^1 X = X + K(z-h(X)) P = (I-KH)P

Definition at line 143 of file ekf_range.c.

References K, EKFRange::P, EKFRange::R_dist, EKFRange::state, EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.

Referenced by check_and_compute_data().

+ Here is the caller graph for this function:

void ekf_range_update_noise ( struct EKFRange ekf_range,
float  Q_sigma2,
float  R_dist,
float  R_speed 
)

Update process and measurement noises.

Parameters
[in]ekf_rangeEKFRange structure
[in]Q_sigma2process noise
[in]R_distmeasurement noise on distance
[in]R_speedmeasurement noise on speed

Definition at line 93 of file ekf_range.c.

References EKFRange::dt, EKF_RANGE_DIM, EKFRange::Q, EKFRange::R_dist, and EKFRange::R_speed.

Referenced by dw1000_arduino_update_ekf_q(), dw1000_arduino_update_ekf_r_dist(), and dw1000_arduino_update_ekf_r_speed().

+ Here is the caller graph for this function:

void ekf_range_update_speed ( struct EKFRange ekf_range,
float  speed,
uint8_t  type 
)

Update step based on speed measure.

Parameters
[in]ekf_rangeEKFRange structure
[in]speednew speed measurement
[in]type1: horizontal ground speed norm, 2: vertical ground speed norm, 3: 3D ground speed norm

Definition at line 191 of file ekf_range.c.