linear_kalman_filter.h

Go to the documentation of this file.

/*
 * Copyright (C) 2021 Gautier Hattenberger <gautier.hattenberger@enac.fr>
 *
 * This file is part of paparazzi
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, see
 * <http://www.gnu.org/licenses/>.
 */
#ifndef LINEAR_KALMAN_FILTER_H
#define LINEAR_KALMAN_FILTER_H
 
#include "std.h"
 
// maximum size for the state vector
#ifndef KF_MAX_STATE_SIZE
#define KF_MAX_STATE_SIZE 6
#endif
 
// maximum size for the command vector
#ifndef KF_MAX_CMD_SIZE
#define KF_MAX_CMD_SIZE 2
#endif
 
// maximum size for the measurement vector
#ifndef KF_MAX_MEAS_SIZE
#define KF_MAX_MEAS_SIZE 6
#endif
 
struct linear_kalman_filter {
  // filled by user after calling init function
  float A[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE];  
  float B[KF_MAX_STATE_SIZE][KF_MAX_CMD_SIZE];    
  float C[KF_MAX_MEAS_SIZE][KF_MAX_STATE_SIZE];   
  float P[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE];  
  float Q[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE];  
  float R[KF_MAX_MEAS_SIZE][KF_MAX_MEAS_SIZE];    
 
  float X[KF_MAX_STATE_SIZE];                     
 
  uint8_t n;  
  uint8_t c;  
  uint8_t m;  
};
 
extern bool linear_kalman_filter_init(struct linear_kalman_filter *filter, uint8_t n, uint8_t c, uint8_t m);
 
extern void linear_kalman_filter_predict(struct linear_kalman_filter *filter, float *U);
 
extern void linear_kalman_filter_update(struct linear_kalman_filter *filter, float *Y);
 
#endif /* DISCRETE_EKF_H */