40 for (
int i = 0; i < m; i++) {
60 if (i < m &&
j <
n_a) {
74void c2d(
int m,
int nA,
int nB,
float **Fx,
float **G,
float dt,
float **phi,
float **
gamma)
200 filter->
Q[4][4] =
powf(0.2, 2);
201 filter->
Q[5][5] =
powf(0.2, 2);
204 filter->
R[0][0] =
powf(0.5, 2);
205 filter->
R[1][1] =
powf(0.2, 2);
206 filter->
R[2][2] =
powf(0.2, 2);
void float_mat_combine(float **a, float **b, float **o, int m, int n_a, int n_b)
void discrete_ekf_no_north_Hx(float *statein, float **output)
void discrete_ekf_no_north_hsym(float *statein, float *output)
void discrete_ekf_no_north_fsym(float *statein, float *input, float *output)
void discrete_ekf_no_north_predict(struct discrete_ekf_no_north *filter, float *U)
void discrete_ekf_no_north_new(struct discrete_ekf_no_north *filter)
void discrete_ekf_no_north_Fx(float *statein, float *input, float **output)
void extractPhiGamma(float **inmat, float **phi, float **gamma, int m, int n_a, int n_b)
void discrete_ekf_no_north_update(struct discrete_ekf_no_north *filter, float *Z)
void discrete_ekf_no_north_G(float *statein, float **output)
void c2d(int m, int nA, int nB, float **Fx, float **G, float dt, float **phi, float **gamma)
float Gamma[EKF_N][EKF_L]
static void float_vect_sum(float *o, const float *a, const float *b, const int n)
o = a + b
static void float_vect_zero(float *a, const int n)
a = 0
static void float_mat_diff(float **o, float **a, float **b, int m, int n)
o = a - b
static void float_mat_sum(float **o, float **a, float **b, int m, int n)
o = a + b
static void float_mat_vect_mul(float *o, float **a, float *b, int m, int n)
o = a * b
static void float_vect_scale(float *a, const float s, const int n)
a *= s
static void float_mat_zero(float **a, int m, int n)
a = 0
static void float_mat_mul(float **o, float **a, float **b, int m, int n, int l)
o = a * b
static void float_vect_diff(float *o, const float *a, const float *b, const int n)
o = a - b
static void float_mat_transpose(float **o, float **a, int n, int m)
transpose non-square matrix
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
Make a pointer to a matrix of _rows lines.
void float_mat_invert(float **o, float **mat, int n)
Calculate inverse of any n x n matrix (passed as C array) o = mat^-1 Algorithm verified with Matlab.
static void float_mat_transpose_square(float **a, int n)
transpose square matrix
void float_mat_exp(float **a, float **o, int n)
static void float_mat_diagonal_scal(float **o, float v, int n)
Make an n x n identity matrix (for matrix passed as array)
static void float_mat_scale(float **a, float k, int m, int n)
a *= k, where k is a scalar value
Paparazzi floating point algebra.