ekf.h

Go to the documentation of this file.

// Copyright 2020-2021 Espressif Systems (Shanghai) PTE LTD
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
 
#ifndef _ekf_h_
#define _ekf_h_
 
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
#include <stdint.h>
#include <mat.h>

class ekf {
public:

    ekf(int x, int w);
 

    virtual ~ekf();
    virtual void Process(float *u, float dt);
 

    virtual void Init() = 0;
    int NUMX;
    int NUMW;

    dspm::Mat &X;

    dspm::Mat &F;
    dspm::Mat &G;

    dspm::Mat &P;

    dspm::Mat &Q;

    void RungeKutta(dspm::Mat &x, float *u, float dt);
 
    // System Dependent methods:

    virtual dspm::Mat StateXdot(dspm::Mat &x, float *u);
    virtual void LinearizeFG(dspm::Mat &x, float *u) = 0;
    //
 
    // System independent methods

    virtual void CovariancePrediction(float dt);

    virtual void Update(dspm::Mat &H, float *measured, float *expected, float *R);
    virtual void UpdateRef(dspm::Mat &H, float *measured, float *expected, float *R);

    float *HP;
    float *Km;
 
public:
    // Additional universal helper methods
    static dspm::Mat quat2rotm(float q[4]);

    static dspm::Mat rotm2quat(dspm::Mat &R);

    static dspm::Mat quat2eul(const float q[4]);
    static dspm::Mat eul2rotm(float xyz[3]);

    static dspm::Mat rotm2eul(dspm::Mat &rotm);

    static dspm::Mat dFdq(dspm::Mat &vector, dspm::Mat &quat);

    static dspm::Mat dFdq_inv(dspm::Mat &vector, dspm::Mat &quat);

    static dspm::Mat SkewSym4x4(float *w);
 
    // q product
    // Rl = [q(1) - q(2) - q(3) - q(4); ...
    //      q(2)  q(1) - q(4)  q(3); ...
    //      q(3)  q(4)  q(1) - q(2); ...
    //      q(4) - q(3)  q(2)  q(1); ...

    static dspm::Mat qProduct(float *q);
 
};
 
#endif // _ekf_h_