37 filter->
P[2][2] = 0.1;
38 filter->
P[3][3] = 0.1;
39 filter->
P[4][4] = 0.1;
40 filter->
P[5][5] = 0.1;
41 filter->
P[6][6] = 0.1;
46 filter->
Q[0][0] = 0.01;
47 filter->
Q[1][1] = 0.01;
51 filter->
R[0][0] = 0.2;
152 dX[0] = -(X[2] - X[4]) * dt;
153 dX[1] = -(X[3] - X[5]) * dt;
167 Z[0] =
sqrt(
pow(X[0], 2.f) +
pow(X[1], 2.f) +
pow(X[6], 2.f));
183 else if (((
row == 1) && (
col == 2)) ||
184 ((
row == 2) && (
col == 3)) ||
185 ((
row == 3) && (
col == 4)) ||
186 ((
row == 4) && (
col == 5)) ||
187 ((
row == 5) && (
col == 6))) {
void discrete_ekf_update(struct discrete_ekf *filter, float *Z)
void discrete_ekf_predict(struct discrete_ekf *filter)
void linear_filter(float *X, float dt, float *dX, float **A)
void discrete_ekf_new(struct discrete_ekf *filter)
void linear_measure(float *X, float *Z, float **H)
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_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
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 struct FloatVect3 H
Paparazzi floating point algebra.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.