33 #define EKF_RANGE_DIM 6
62 extern 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);
void ekf_range_update_noise(struct EKFRange *ekf_range, float Q_sigma2, float R_dist, float R_speed)
Update process and measurement noises.
void ekf_range_set_state(struct EKFRange *ekf_range, struct EnuCoor_f pos, struct EnuCoor_f speed)
Set initial state vector.
float state[EKF_RANGE_DIM]
state vector
struct EnuCoor_f ekf_range_get_speed(struct EKFRange *ekf_range)
Get current speed.
float R_dist
measurement noise on distances (assumed the same for all anchors)
void ekf_range_update_dist(struct EKFRange *ekf_range, float dist, struct EnuCoor_f anchor)
Update step based on each new distance data.
float P[EKF_RANGE_DIM][EKF_RANGE_DIM]
covariance matrix
void ekf_range_update_speed(struct EKFRange *ekf_range, float speed, uint8_t type)
Update step based on speed measure.
float R_speed
measurement noise on speed (assumed the same for all axis)
float Q[EKF_RANGE_DIM][EKF_RANGE_DIM]
process noise matrix
float dt
prediction step (in seconds)
void ekf_range_get_state(struct EKFRange *ekf_range, struct EnuCoor_f *pos, struct EnuCoor_f *speed)
Get current state.
struct EnuCoor_f ekf_range_get_pos(struct EKFRange *ekf_range)
Get current pos.
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.
void ekf_range_predict(struct EKFRange *ekf_range)
Prediction step.
Paparazzi floating point algebra.
Paparazzi floating point math for geodetic calculations.
vector in East North Up coordinates Units: meters
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.