kalman_filter.hpp

Go to the documentation of this file.

/*****************************************************************************/
/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
/*****************************************************************************/
 
/*
 * Copyright (c) 2020-2021 Advanced Robotics at the University of Washington <robomstr@uw.edu>
 *
 * This file is part of Taproot.
 *
 * Taproot 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 3 of the License, or
 * (at your option) any later version.
 *
 * Taproot 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 Taproot.  If not, see <https://www.gnu.org/licenses/>.
 */
 
#ifndef TAPROOT_KALMAN_FILTER_HPP_
#define TAPROOT_KALMAN_FILTER_HPP_
 
#include <cinttypes>
 
#include "modm/architecture/interface/assert.h"
#include "modm/math/matrix.hpp"
 
#include "cmsis_mat.hpp"
 
namespace tap::algorithms
{
template <uint16_t STATES, uint16_t INPUTS>
class KalmanFilter
{
public:
    KalmanFilter(
        const float (&A)[STATES * STATES],
        const float (&C)[INPUTS * STATES],
        const float (&Q)[STATES * STATES],
        const float (&R)[INPUTS * INPUTS],
        const float (&P0)[STATES * STATES])
        : A(A),
          At(),
          C(C),
          Ct(),
          Q(Q),
          R(R),
          xHat(),
          P(P0),
          P0(P0),
          K(),
          I()
    {
        arm_mat_trans_f32(&this->A.matrix, &At.matrix);
        arm_mat_trans_f32(&this->C.matrix, &Ct.matrix);
        I.constructIdentityMatrix();
    }
 
    void init(const float (&initialX)[STATES * 1])
    {
        xHat.copyData(initialX);
        P.data = P0.data;
        initialized = true;
    }
 
    void performUpdate(const CMSISMat<INPUTS, 1> &y)
    {
        if (!initialized)
        {
            return;
        }
 
        // Predict state
        // TODO add control vector if necessary in the future
        xHat = A * xHat;
        P = A * P * At + Q;
 
        // Update step
        K = P * Ct * (C * P * Ct + R).inverse();
        xHat = xHat + K * (y - C * xHat);
        P = (I - K * C) * P;
    }
 
    const std::array<float, STATES> &getStateVectorAsMatrix() const { return xHat.data; }
 
    inline std::array<float, INPUTS * INPUTS> &getMeasurementCovariance() { return R.data; }
 
private:
    const CMSISMat<STATES, STATES> A;
 
    CMSISMat<STATES, STATES> At;
 
    const CMSISMat<INPUTS, STATES> C;
 
    CMSISMat<STATES, INPUTS> Ct;
 
    const CMSISMat<STATES, STATES> Q;
    CMSISMat<INPUTS, INPUTS> R;
 
    CMSISMat<STATES, 1> xHat;
 
    CMSISMat<STATES, STATES> P;
 
    CMSISMat<STATES, STATES> P0;
 
    CMSISMat<STATES, INPUTS> K;
 
    CMSISMat<STATES, STATES> I;
 
    bool initialized = false;
};
 
}  // namespace tap::algorithms
 
#endif  // TAPROOT_KALMAN_FILTER_HPP_