35 const float dt2 = dt * dt;
36 const float dt3 = dt2 * dt / 2.f;
37 const float dt4 = dt2 * dt2 / 4.f;
47 kalman->
P[i + 1][i + 1] = P0_speed;
48 kalman->
Q[i][i] = Q_sigma2 * dt4;
49 kalman->
Q[i + 1][i] = Q_sigma2 * dt3;
50 kalman->
Q[i][i + 1] = Q_sigma2 * dt3;
51 kalman->
Q[i + 1][i + 1] = Q_sigma2 * dt2;
67 }
else if (i % 2 == 0 && j == i + 1) {
136 const float dt2 = dt * dt;
137 const float dt3 = dt2 * dt / 2.f;
138 const float dt4 = dt2 * dt2 / 4.f;
140 kalman->
Q[i][i] = Q_sigma2 * dt4;
141 kalman->
Q[i + 1][i] = Q_sigma2 * dt3;
142 kalman->
Q[i][i + 1] = Q_sigma2 * dt3;
143 kalman->
Q[i + 1][i + 1] = Q_sigma2 * dt2;
194 if (fabsf(S[0][0]) + fabsf(S[1][1]) + fabsf(S[2][2]) < 1e-5) {
202 float HinvS_tmp[6][3];
228 for (
int i = 0; i < 6; i++) {
229 for (
int j = 0; j < 6; j++) {
234 for (
int i = 0; i < 6; i++) {
235 for (
int j = 0; j < 6; j++) {
236 for (
int k = 0; k < 6; k++) {
237 kalman->
P[i][j] -= KH_tmp[i][k] * P_tmp[k][j];
252 float Z[3] = { pos.
x, pos.
y, pos.
z };
269 float Z[3] = { speed.
x, speed.
y, speed.
z };
static void float_vect_add(float *a, const float *b, const int n)
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 simple_kinematic_kalman_update(struct SimpleKinematicKalman *kalman, float **_H, float *Z)
generic correction step
struct FloatVect3 simple_kinematic_kalman_get_pos(struct SimpleKinematicKalman *kalman)
Get current pos.
void simple_kinematic_kalman_get_state(struct SimpleKinematicKalman *kalman, struct FloatVect3 *pos, struct FloatVect3 *speed)
Get current state.
void simple_kinematic_kalman_update_noise(struct SimpleKinematicKalman *kalman, float Q_sigma2, float r)
Update process and measurement noises.
void simple_kinematic_kalman_predict(struct SimpleKinematicKalman *kalman)
propagate dynamic model
void simple_kinematic_kalman_set_state(struct SimpleKinematicKalman *kalman, struct FloatVect3 pos, struct FloatVect3 speed)
Set initial state vector.
void simple_kinematic_kalman_update_pos(struct SimpleKinematicKalman *kalman, struct FloatVect3 pos)
correction step for position
void simple_kinematic_kalman_init(struct SimpleKinematicKalman *kalman, float P0_pos, float P0_speed, float Q_sigma2, float r, float dt)
Init SimpleKinematicKalman internal struct.
void simple_kinematic_kalman_update_speed(struct SimpleKinematicKalman *kalman, struct FloatVect3 speed, uint8_t type)
correction step for speed
struct FloatVect3 simple_kinematic_kalman_get_speed(struct SimpleKinematicKalman *kalman)
Get current speed.
#define SIMPLE_KINEMATIC_KALMAN_DIM
#define SIMPLE_KINEMATIC_KALMAN_SPEED_HORIZONTAL
float Hp[SIMPLE_KINEMATIC_KALMAN_DIM/2][SIMPLE_KINEMATIC_KALMAN_DIM]
observation matrix for position
float r
measurement noise (assumed the same for all anchors)
float state[SIMPLE_KINEMATIC_KALMAN_DIM]
state vector
float Q[SIMPLE_KINEMATIC_KALMAN_DIM][SIMPLE_KINEMATIC_KALMAN_DIM]
process noise matrix
float P[SIMPLE_KINEMATIC_KALMAN_DIM][SIMPLE_KINEMATIC_KALMAN_DIM]
covariance matrix
#define SIMPLE_KINEMATIC_KALMAN_SPEED_VERTICAL
float F[SIMPLE_KINEMATIC_KALMAN_DIM][SIMPLE_KINEMATIC_KALMAN_DIM]
dynamic matrix
float dt
prediction step (in seconds)
float Hs[SIMPLE_KINEMATIC_KALMAN_DIM/2][SIMPLE_KINEMATIC_KALMAN_DIM]
observation matrix for speed
static struct SimpleKinematicKalman kalman
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.