discrete_ekf_no_north.c File Reference

#include "modules/relative_localization_filter/discrete_ekf_no_north.h"
#include "math/pprz_algebra_float.h"
#include <math.h>
#include <stdio.h>

Include dependency graph for discrete_ekf_no_north.c:

Go to the source code of this file.

Enumerations
enum	ekf_statein {   x12, y12, z1, z2,   u1, v1, u2, v2,   gam }
enum	ekf_input {   u1dm, v1dm, u2dm, v2dm,   r1m, r2m }

Functions
void	extractPhiGamma (float **inmat, float **phi, float **gamma, int m, int n_a, int n_b)
void	float_mat_combine (float **a, float **b, float **o, int m, int n_a, int n_b)
void	c2d (int m, int nA, int nB, float **Fx, float **G, float dt, float **phi, float **gamma)
void	discrete_ekf_no_north_fsym (float *statein, float *input, float *output)
void	discrete_ekf_no_north_hsym (float *statein, float *output)
void	discrete_ekf_no_north_Fx (float *statein, float *input, float **output)
void	discrete_ekf_no_north_G (float *statein, float **output)
void	discrete_ekf_no_north_Hx (float *statein, float **output)
void	discrete_ekf_no_north_new (struct discrete_ekf_no_north *filter)
void	discrete_ekf_no_north_predict (struct discrete_ekf_no_north *filter, float *U)
void	discrete_ekf_no_north_update (struct discrete_ekf_no_north *filter, float *Z)

Detailed Description

Author

Steven van der Helm, Mario Coppola Discrete Extended Kalman Filter for Relative Localization without requiring knowledge of North This implementation is from van der Helm et al. "On-board Range-based Relative Localization for Micro Aerial Vehicles in indoor Leader-Follower Flight." (2018). Available at https://arxiv.org/pdf/1805.07171.pdf

Definition in file discrete_ekf_no_north.c.

Enumeration Type Documentation

ekf_input

enum ekf_input

Enumerator
u1dm
v1dm
u2dm
v2dm
r1m
r2m

Definition at line 35 of file discrete_ekf_no_north.c.

ekf_statein

enum ekf_statein

Enumerator
x12
y12
z1
z2
u1
v1
u2
v2
gam

Definition at line 34 of file discrete_ekf_no_north.c.

Function Documentation

c2d()

void c2d	(	int	m,
		int	nA,
		int	nB,
		float **	Fx,
		float **	G,
		float	dt,
		float **	phi,
		float **	gamma
	)

Definition at line 74 of file discrete_ekf_no_north.c.

References EKF_M, EKF_N, extractPhiGamma(), float_mat_combine(), float_mat_exp(), float_mat_scale(), and MAKE_MATRIX_PTR.

Referenced by discrete_ekf_no_north_predict().

Here is the call graph for this function:

Here is the caller graph for this function:

discrete_ekf_no_north_fsym()

void discrete_ekf_no_north_fsym	(	float *	statein,
		float *	input,
		float *	output
	)

Definition at line 101 of file discrete_ekf_no_north.c.

References gam, r1m, r2m, u1, u1dm, u2, u2dm, v1, v1dm, v2, v2dm, x12, and y12.

Referenced by discrete_ekf_no_north_predict().

Here is the caller graph for this function:

discrete_ekf_no_north_Fx()

void discrete_ekf_no_north_Fx	(	float *	statein,
		float *	input,
		float **	output
	)

Definition at line 130 of file discrete_ekf_no_north.c.

References EKF_N, float_mat_zero(), gam, MAKE_MATRIX_PTR, r1m, r2m, u2, and v2.

Referenced by discrete_ekf_no_north_predict().

Here is the call graph for this function:

Here is the caller graph for this function:

discrete_ekf_no_north_G()

void discrete_ekf_no_north_G	(	float *	statein,
		float **	output
	)

Definition at line 150 of file discrete_ekf_no_north.c.

References EKF_L, EKF_N, float_mat_zero(), MAKE_MATRIX_PTR, u1, u2, v1, v2, x12, and y12.

Referenced by discrete_ekf_no_north_predict().

Here is the call graph for this function:

Here is the caller graph for this function:

discrete_ekf_no_north_hsym()

void discrete_ekf_no_north_hsym	(	float *	statein,
		float *	output
	)

Definition at line 119 of file discrete_ekf_no_north.c.

References u1, u2, v1, v2, x12, y12, z1, and z2.

Referenced by discrete_ekf_no_north_predict().

Here is the caller graph for this function:

discrete_ekf_no_north_Hx()

void discrete_ekf_no_north_Hx	(	float *	statein,
		float **	output
	)

Definition at line 168 of file discrete_ekf_no_north.c.

References EKF_M, EKF_N, float_mat_zero(), MAKE_MATRIX_PTR, x12, y12, z1, and z2.

Referenced by discrete_ekf_no_north_predict().

Here is the call graph for this function:

Here is the caller graph for this function:

discrete_ekf_no_north_new()

void discrete_ekf_no_north_new

(

struct discrete_ekf_no_north *

filter

)

Definition at line 192 of file discrete_ekf_no_north.c.

References discrete_ekf_no_north::dt, EKF_M, EKF_N, float_mat_diagonal_scal(), float_vect_zero(), MAKE_MATRIX_PTR, discrete_ekf_no_north::P, discrete_ekf_no_north::Q, discrete_ekf_no_north::R, and discrete_ekf_no_north::X.

Referenced by range_msg_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

discrete_ekf_no_north_predict()

void discrete_ekf_no_north_predict	(	struct discrete_ekf_no_north *	filter,
		float *	U
	)

Definition at line 219 of file discrete_ekf_no_north.c.

References c2d(), discrete_ekf_no_north_fsym(), discrete_ekf_no_north_Fx(), discrete_ekf_no_north_G(), discrete_ekf_no_north_hsym(), discrete_ekf_no_north_Hx(), discrete_ekf_no_north::dt, EKF_L, EKF_N, float_mat_mul(), float_mat_sum(), float_mat_transpose(), float_mat_transpose_square(), float_vect_scale(), float_vect_sum(), discrete_ekf_no_north::Fx, discrete_ekf_no_north::G, discrete_ekf_no_north::Gamma, discrete_ekf_no_north::H, MAKE_MATRIX_PTR, discrete_ekf_no_north::P, discrete_ekf_no_north::Phi, discrete_ekf_no_north::Q, discrete_ekf_no_north::tmp1, discrete_ekf_no_north::tmp2, discrete_ekf_no_north::tmp3, discrete_ekf_no_north::tmp4, discrete_ekf_no_north::X, discrete_ekf_no_north::Xp, and discrete_ekf_no_north::Zp.

Referenced by range_msg_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

discrete_ekf_no_north_update()

void discrete_ekf_no_north_update	(	struct discrete_ekf_no_north *	filter,
		float *	Z
	)

Definition at line 270 of file discrete_ekf_no_north.c.

References EKF_M, EKF_N, float_mat_diff(), float_mat_invert(), float_mat_mul(), float_mat_sum(), float_mat_transpose(), float_mat_vect_mul(), float_vect_diff(), float_vect_sum(), discrete_ekf_no_north::H, discrete_ekf_no_north::Ht, MAKE_MATRIX_PTR, discrete_ekf_no_north::P, discrete_ekf_no_north::R, discrete_ekf_no_north::tmp1, discrete_ekf_no_north::tmp2, discrete_ekf_no_north::tmp3, discrete_ekf_no_north::X, discrete_ekf_no_north::Xp, mesonh.mesonh_atmosphere::Z, and discrete_ekf_no_north::Zp.

Referenced by range_msg_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

extractPhiGamma()

void extractPhiGamma	(	float **	inmat,
		float **	phi,
		float **	gamma,
		int	m,
		int	n_a,
		int	n_b
	)

Definition at line 37 of file discrete_ekf_no_north.c.

Referenced by c2d().

Here is the caller graph for this function:

float_mat_combine()

void float_mat_combine	(	float **	a,
		float **	b,
		float **	o,
		int	m,
		int	n_a,
		int	n_b
	)

Definition at line 55 of file discrete_ekf_no_north.c.

References b.

Referenced by c2d().

Here is the caller graph for this function: