TinyEKF.h

Go to the documentation of this file.

/*
 * TinyEKF: Extended Kalman Filter for Arduino and TeensyBoard.
 *
 * Copyright (C) 2015 Simon D. Levy
 *
 * MIT License
 */
 
#include <stdio.h>
#include <stdlib.h>
#include "tiny_ekf_struct.h"
 
void ekf_init(void *, int, int);
int ekf_step(void *, double *);
 
class TinyEKF {
 
private:
 
    ekf_t ekf;
 
protected:
 
    double * x;
 
    TinyEKF() {
        ekf_init(&this->ekf, Nsta, Mobs);
        this->x = this->ekf.x;
    }
 
    ~TinyEKF() { }
 
    virtual void model(double fx[Nsta], double F[Nsta][Nsta], double hx[Mobs], double H[Mobs][Nsta]) = 0;
 
    void setP(int i, int j, double value)
    {
        this->ekf.P[i][j] = value;
    }
 
    void setQ(int i, int j, double value)
    {
        this->ekf.Q[i][j] = value;
    }
 
    void setR(int i, int j, double value)
    {
        this->ekf.R[i][j] = value;
    }
 
public:
 
    double getX(int i)
    {
        return this->ekf.x[i];
    }
 
    void setX(int i, double value)
    {
        this->ekf.x[i] = value;
    }
 
    bool step(double * z)
    {
        this->model(this->ekf.fx, this->ekf.F, this->ekf.hx, this->ekf.H);
 
        return ekf_step(&this->ekf, z) ? false : true;
    }
};