KalmanFilter.hpp

Go to the documentation of this file.

// This file is part of INSTINCT, the INS Toolkit for Integrated
// Navigation Concepts and Training by the Institute of Navigation of
// the University of Stuttgart, Germany.
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at https://mozilla.org/MPL/2.0/.
 
 
#pragma once
 
#include <Eigen/Core>
#include <Eigen/Dense>
 
#include "Navigation/Math/Math.hpp"
 
namespace NAV
{
class KalmanFilter
{
  public:
    KalmanFilter(int n, int m)
    {
        // x̂ State vector
        x = Eigen::MatrixXd::Zero(n, 1);
 
        // 𝐏 Error covariance matrix
        P = Eigen::MatrixXd::Zero(n, n);
 
        // 𝚽 State transition matrix
        Phi = Eigen::MatrixXd::Zero(n, n);
 
        // 𝐐 System/Process noise covariance matrix
        Q = Eigen::MatrixXd::Zero(n, n);
 
        // 𝐳 Measurement vector
        z = Eigen::MatrixXd::Zero(m, 1);
 
        // 𝐇 Measurement sensitivity Matrix
        H = Eigen::MatrixXd::Zero(m, n);
 
        // 𝐑 = 𝐸{𝐰ₘ𝐰ₘᵀ} Measurement noise covariance matrix
        R = Eigen::MatrixXd::Zero(m, m);
 
        // 𝗦 Measurement prediction covariance matrix
        S = Eigen::MatrixXd::Zero(m, m);
 
        // 𝐊 Kalman gain matrix
        K = Eigen::MatrixXd::Zero(n, m);
 
        // 𝑰 Identity Matrix
        I = Eigen::MatrixXd::Identity(n, n);
    }
 
    KalmanFilter() = delete;
 
    void setZero()
    {
        // x̂ State vector
        x.setZero();
 
        // 𝐏 Error covariance matrix
        P.setZero();
 
        // 𝚽 State transition matrix
        Phi.setZero();
 
        // 𝐐 System/Process noise covariance matrix
        Q.setZero();
 
        // 𝐳 Measurement vector
        z.setZero();
 
        // 𝐇 Measurement sensitivity Matrix
        H.setZero();
 
        // 𝐑 = 𝐸{𝐰ₘ𝐰ₘᵀ} Measurement noise covariance matrix
        R.setZero();
 
        // 𝗦 Measurement prediction covariance matrix
        S.setZero();
 
        // 𝐊 Kalman gain matrix
        K.setZero();
    }
 
    void predict()
    {
        // Math: \mathbf{\hat{x}}_k^- = \mathbf{\Phi}_{k-1}\mathbf{\hat{x}}_{k-1}^+ \qquad \text{P. Groves}\,(3.14)
        x = Phi * x;
 
        // Math: \mathbf{P}_k^- = \mathbf{\Phi}_{k-1} P_{k-1}^+ \mathbf{\Phi}_{k-1}^T + \mathbf{Q}_{k-1} \qquad \text{P. Groves}\,(3.15)
        P = Phi * P * Phi.transpose() + Q;
    }
 
    void correct()
    {
        S = H * P * H.transpose() + R;
 
        // Math: \mathbf{K}_k = \mathbf{P}_k^- \mathbf{H}_k^T (\mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^T + R_k)^{-1} \qquad \text{P. Groves}\,(3.21)
        K = P * H.transpose() * S.inverse();
 
        // Math: \begin{align*} \mathbf{\hat{x}}_k^+ &= \mathbf{\hat{x}}_k^- + \mathbf{K}_k (\mathbf{z}_k - \mathbf{H}_k \mathbf{\hat{x}}_k^-) \\ &= \mathbf{\hat{x}}_k^- + \mathbf{K}_k \mathbf{\delta z}_k^{-} \end{align*} \qquad \text{P. Groves}\,(3.24)
        x = x + K * (z - H * x);
 
        // Math: \mathbf{P}_k^+ = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_k^- \qquad \text{P. Groves}\,(3.25)
        P = (I - K * H) * P;
    }
 
    void correctWithMeasurementInnovation()
    {
        // Math: \mathbf{K}_k = \mathbf{P}_k^- \mathbf{H}_k^T (\mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^T + R_k)^{-1} \qquad \text{P. Groves}\,(3.21)
        K = P * H.transpose() * (H * P * H.transpose() + R).inverse();
 
        // Math: \begin{align*} \mathbf{\hat{x}}_k^+ &= \mathbf{\hat{x}}_k^- + \mathbf{K}_k (\mathbf{z}_k - \mathbf{H}_k \mathbf{\hat{x}}_k^-) \\ &= \mathbf{\hat{x}}_k^- + \mathbf{K}_k \mathbf{\delta z}_k^{-} \end{align*} \qquad \text{P. Groves}\,(3.24)
        x = x + K * z;
 
        // Math: \mathbf{P}_k^+ = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_k^- \qquad \text{P. Groves}\,(3.25)
        // P = (I - K * H) * P;
 
        // Math: \mathbf{P}_k^+ = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_k^- (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k)^T + \mathbf{K}_k \mathbf{R}_k \mathbf{K}_k^T \qquad \text{Brown & Hwang}\,(p. 145, eq. 4.2.11)
        P = (I - K * H) * P * (I - K * H).transpose() + K * R * K.transpose();
    }
 
    static Eigen::MatrixXd transitionMatrix(const Eigen::MatrixXd& F, double tau_s)
    {
        // Transition matrix 𝚽
        return Eigen::MatrixXd::Identity(F.rows(), F.cols()) + F * tau_s;
    }
 
    Eigen::MatrixXd x;
 
    Eigen::MatrixXd P;
 
    Eigen::MatrixXd Phi;
 
    Eigen::MatrixXd Q;
 
    Eigen::MatrixXd z;
 
    Eigen::MatrixXd H;
 
    Eigen::MatrixXd R;
 
    Eigen::MatrixXd S;
 
    Eigen::MatrixXd K;
 
  private:
    Eigen::MatrixXd I;
};
 
template<typename Derived>
typename Derived::PlainObject transitionMatrix_Phi_Taylor(const Eigen::MatrixBase<Derived>& F, double tau_s, size_t order)
{
    // Transition matrix 𝚽
    typename Derived::PlainObject Phi;
 
    if constexpr (Derived::RowsAtCompileTime == Eigen::Dynamic)
    {
        Phi = Eigen::MatrixBase<Derived>::Identity(F.rows(), F.cols());
    }
    else
    {
        Phi = Eigen::MatrixBase<Derived>::Identity();
    }
    // std::cout << "Phi = I";
 
    for (size_t i = 1; i <= order; i++)
    {
        typename Derived::PlainObject Fpower = F;
        // std::cout << " + (F";
 
        for (size_t j = 1; j < i; j++) // F^j
        {
            // std::cout << "*F";
            Fpower *= F;
        }
        // std::cout << "*tau_s^" << i << ")";
        // std::cout << "/" << math::factorial(i);
        Phi += (Fpower * std::pow(tau_s, i)) / math::factorial(i);
    }
    // std::cout << "\n";
 
    return Phi;
}
 
template<typename Derived>
typename Derived::PlainObject transitionMatrix_Phi_exp(const Eigen::MatrixBase<Derived>& F, typename Derived::Scalar tau_s)
{
    // Transition matrix 𝚽
    return (F * tau_s).exp();
}
 
} // namespace NAV