altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

EKF.h

Committer:
pmic
Date:
2020-01-27
Revision:
30:9b0cd3caf0ec
Parent:
21:31e01d3e0143

File content as of revision 30:9b0cd3caf0ec:


#ifndef EKF_H_
#define EKF_H_

#include <mbed.h>
#include "matrix.h"

class EKF
{
public:

    EKF(float);
    virtual ~EKF();
    float get_est_state(uint8_t);
    void loop(matrix *);
    void display_matrix(char);
    float getYaw(void){
        return 0;
        }
    void update(float,float,float,float,float,float);
private:
    matrix x;
    matrix x_km;
    matrix P;
    matrix Q;
    matrix R;
    matrix F;
    matrix H;
    matrix K;
    matrix E;
    float g,tau_g,m,itau_g,k1,kdm;
    float Ts;
    void dgl(matrix *);
    float my_sin(float x){return x;}
    float my_cos(float x){return 1.0f-0.5f*x*x;}
};

#endif