kalman.h

#ifndef _AVRIO_KALMAN_H_
#define _AVRIO_KALMAN_H_

#include <avrio/defs.h>

__BEGIN_C_DECLS
/* ========================================================================== */
/* constants ================================================================ */
#define KALMAN_Q_ANGLE    0.001
#define KALMAN_Q_BIAS     0.003
#define KALMAN_R_MEASURE  0.03

/* structures =============================================================== */
typedef struct xKalmanFilter {

  double fQAngle; 
  double fQBias; 
  double fRMeasure; 
  
  double fAngle; 
  double fBias; 
  double fRate; 
  
  double fP[2][2]; 
  double fK[2]; 
  double fY; 
  double fS; 
} xKalmanFilter;


/* internal public functions ================================================ */
void vKalmanInit (xKalmanFilter * xFilter);

void vKalmanProcess (xKalmanFilter * xFilter, double fNewAngle, double fNewRate, uint16_t usDt);

// -----------------------------------------------------------------------------

#  if defined(__DOXYGEN__)
/* 
 * __DOXYGEN__ defined
 * Partie documentation ne devant pas être compilée.
 * =============================================================================
 */
#  else
/* 
 * __DOXYGEN__ not defined
 * Partie ne devant pas être documentée.
 * =============================================================================
 */

/* public variables ========================================================= */

#  endif /* __DOXYGEN__ not defined */
/* ========================================================================== */
__END_C_DECLS
#endif  /* _AVRIO_KALMAN_H_ not defined */