Paparazzi UAS
v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
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... | |
Definition in file ekf_range.c.
Get current pos.
[in] | ekf_range | EKFRange structure |
Definition at line 65 of file ekf_range.c.
References EKFRange::state, EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.
Referenced by check_and_compute_data(), and dw1000_arduino_periodic().
Get current speed.
[in] | ekf_range | EKFRange structure |
Definition at line 65 of file ekf_range.c.
Referenced by check_and_compute_data(), and dw1000_arduino_periodic().
void ekf_range_get_state | ( | struct EKFRange * | ekf_range, |
struct EnuCoor_f * | pos, | ||
struct EnuCoor_f * | speed | ||
) |
Get current state.
[in] | ekf_range | EKFRange structure |
[out] | pos | current position |
[out] | speed | current speed |
Definition at line 65 of file ekf_range.c.
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.
[in] | ekf_range | EKFRange structure |
[in] | P0_pos | initial covariance on position |
[in] | P0_speed | initial covariance on speed |
[in] | Q_sigma2 | process noise |
[in] | R_dist | measurement noise on distance |
[in] | R_speed | measurement noise on speed |
[in] | dt | prediction 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().
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().
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
[in] | ekf_range | EKFRange structure |
[in] | pos | initial position |
[in] | speed | initial 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().
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 if(), K, EKFRange::P, EKFRange::R_dist, EKFRange::state, EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.
Referenced by check_and_compute_data().
void ekf_range_update_noise | ( | struct EKFRange * | ekf_range, |
float | Q_sigma2, | ||
float | R_dist, | ||
float | R_speed | ||
) |
Update process and measurement noises.
[in] | ekf_range | EKFRange structure |
[in] | Q_sigma2 | process noise |
[in] | R_dist | measurement noise on distance |
[in] | R_speed | measurement 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().
Update step based on speed measure.
[in] | ekf_range | EKFRange structure |
[in] | speed | new speed measurement |
[in] | type | 1: horizontal ground speed norm, 2: vertical ground speed norm, 3: 3D ground speed norm |
Definition at line 191 of file ekf_range.c.