kalman.h

 
#ifndef RC_KALMAN_H
#define RC_KALMAN_H
 
#ifdef __cplusplus
extern "C" {
#endif
 
#include <stdint.h>
#include <rc/math/vector.h>
#include <rc/math/matrix.h>
 
 
/*
 * @brief      Struct to contain full state of kalman filter
 */
typedef struct rc_kalman_t {
    rc_matrix_t F;      
    rc_matrix_t G;      
    rc_matrix_t H;      
 
    rc_matrix_t Q;      
    rc_matrix_t R;      
    rc_matrix_t P;      
    rc_matrix_t Pi;     
 
    rc_vector_t x_est;  
    rc_vector_t x_pre;  
 
    int initialized;    
    uint64_t step;      
} rc_kalman_t;
 
#define RC_KALMAN_INITIALIZER {\
    .F = RC_MATRIX_INITIALIZER,\
    .G = RC_MATRIX_INITIALIZER,\
    .H = RC_MATRIX_INITIALIZER,\
    .Q = RC_MATRIX_INITIALIZER,\
    .R = RC_MATRIX_INITIALIZER,\
    .P = RC_MATRIX_INITIALIZER,\
    .Pi = RC_MATRIX_INITIALIZER,\
    .x_est = RC_VECTOR_INITIALIZER,\
    .x_pre = RC_VECTOR_INITIALIZER,\
    .initialized = 0,\
    .step = 0}
 
rc_kalman_t rc_kalman_empty(void);
 
int rc_kalman_alloc_lin(rc_kalman_t* kf, rc_matrix_t F, rc_matrix_t G, rc_matrix_t H, rc_matrix_t Q, rc_matrix_t R, rc_matrix_t Pi);
 
int rc_kalman_alloc_ekf(rc_kalman_t* kf, rc_matrix_t Q, rc_matrix_t R, rc_matrix_t Pi);
 
 
int rc_kalman_free(rc_kalman_t* kf);
 
 
int rc_kalman_reset(rc_kalman_t* kf);
 
 
int rc_kalman_update_lin(rc_kalman_t* kf, rc_vector_t u, rc_vector_t y);
 
 
int rc_kalman_update_ekf(rc_kalman_t* kf, rc_matrix_t F, rc_matrix_t H, rc_vector_t x_pre,  rc_vector_t y, rc_vector_t h);
 
 
#ifdef __cplusplus
}
#endif
 
#endif // RC_KALMAN_H