simple_kinematic_kalman.h

Go to the documentation of this file.

/*
 * Copyright (C) 2020 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 SIMPLE_KINEMATIC_KALMAN_H
#define SIMPLE_KINEMATIC_KALMAN_H
 
#include "std.h"
#include "math/pprz_algebra_float.h"
 
#define SIMPLE_KINEMATIC_KALMAN_DIM 6
 
// Speed update types
#define SIMPLE_KINEMATIC_KALMAN_SPEED_HORIZONTAL  1
#define SIMPLE_KINEMATIC_KALMAN_SPEED_VERTICAL    2
#define SIMPLE_KINEMATIC_KALMAN_SPEED_3D          3
 
struct SimpleKinematicKalman {
  float state[SIMPLE_KINEMATIC_KALMAN_DIM];                               
  float P[SIMPLE_KINEMATIC_KALMAN_DIM][SIMPLE_KINEMATIC_KALMAN_DIM];      
  float Q[SIMPLE_KINEMATIC_KALMAN_DIM][SIMPLE_KINEMATIC_KALMAN_DIM];      
  float F[SIMPLE_KINEMATIC_KALMAN_DIM][SIMPLE_KINEMATIC_KALMAN_DIM];      
  float Hp[SIMPLE_KINEMATIC_KALMAN_DIM / 2][SIMPLE_KINEMATIC_KALMAN_DIM]; 
  float Hs[SIMPLE_KINEMATIC_KALMAN_DIM / 2][SIMPLE_KINEMATIC_KALMAN_DIM]; 
  float r;                                                                
  float dt;                                                               
};
 
extern void simple_kinematic_kalman_init(struct SimpleKinematicKalman *kalman, float P0_pos, float P0_speed,
    float Q_sigma2, float r, float dt);
 
extern void simple_kinematic_kalman_set_state(struct SimpleKinematicKalman *kalman, struct FloatVect3 pos,
    struct FloatVect3 speed);
 
extern void simple_kinematic_kalman_get_state(struct SimpleKinematicKalman *kalman, struct FloatVect3 *pos,
    struct FloatVect3 *speed);
 
extern struct FloatVect3 simple_kinematic_kalman_get_pos(struct SimpleKinematicKalman *kalman);
 
extern struct FloatVect3 simple_kinematic_kalman_get_speed(struct SimpleKinematicKalman *kalman);
 
extern void simple_kinematic_kalman_update_noise(struct SimpleKinematicKalman *kalman, float Q_sigma2, float r);
 
extern void simple_kinematic_kalman_predict(struct SimpleKinematicKalman *kalman);
 
extern void simple_kinematic_kalman_update_pos(struct SimpleKinematicKalman *kalman, struct FloatVect3 pos);
 
extern void simple_kinematic_kalman_update_speed(struct SimpleKinematicKalman *kalman, struct FloatVect3 speed, uint8_t type);
 
#endif