StateSpaceUtil.h

#pragma once
 
#include <Eigen/Core>
#include <Eigen/LU>
#include <unsupported/Eigen/MatrixFunctions>

namespace filters::statespace {

template <int size>
Eigen::Matrix<double, size, size>
createCovarianceMatrix(const Eigen::Matrix<double, size, 1>& stdDevs) {
    Eigen::Matrix<double, size, 1> variances = stdDevs.array().square();
    Eigen::Matrix<double, size, size> mat = variances.asDiagonal();
    return mat;
}

template <int numStates>
Eigen::Matrix<double, numStates, numStates>
discreteToContinuous(const Eigen::Matrix<double, numStates, numStates>& A, double dt) {
    return A.log() / dt;
}

template <int numStates, int numInputs>
void discreteToContinuous(Eigen::Matrix<double, numStates, numStates>& A,
                          Eigen::Matrix<double, numStates, numInputs>& B, double dt) {
    // Reference paper: https://doi.org/10.1016/0307-904X(80)90177-8
    auto discA = A;
    auto discB = B;
    A = discA.log() / dt;
    B = A * (discA - Eigen::Matrix<double, numStates, numStates>::Identity()).inverse() *
        discB;
}

template <int numStates, int numInputs>
void continuousToDiscrete(Eigen::Matrix<double, numStates, numStates>& A,
                          Eigen::Matrix<double, numStates, numInputs>& B, double dt) {
    // zero order hold discretization for system and input matrices
    // reference:
    // https://en.wikipedia.org/wiki/Discretization#Discretization_of_linear_state_space_models
    Eigen::Matrix<double, numStates + numInputs, numStates + numInputs> M;
    M.setZero();
    M.template block<numStates, numStates>(0, 0) = A;
    M.template block<numStates, numInputs>(0, numStates) = B;
    M *= dt;
 
    Eigen::Matrix<double, numStates + numInputs, numStates + numInputs> exp = M.exp();
    A = exp.template block<numStates, numStates>(0, 0);
    B = exp.template block<numStates, numInputs>(0, numStates);
}

template <int numStates>
Eigen::Matrix<double, numStates, numStates>
discretizeA(const Eigen::Matrix<double, numStates, numStates>& contA, double dt) {
    // Derived from equations in discreteToContinuous()
    return (contA * dt).exp();
}

template <int numStates>
void discretizeAQ(const Eigen::Matrix<double, numStates, numStates>& contA,
                  const Eigen::Matrix<double, numStates, numStates>& contQ,
                  Eigen::Matrix<double, numStates, numStates>& discA,
                  Eigen::Matrix<double, numStates, numStates>& discQ, double dt) {
    // zero order hold discretization of the system and state covariance matrices
    // reference:
    // https://en.wikipedia.org/wiki/Discretization#Discretization_of_process_noise
    Eigen::Matrix<double, numStates * 2, numStates * 2> M;
    M.template block<numStates, numStates>(0, 0) = -contA;
    M.template block<numStates, numStates>(0, numStates) = contQ;
    M.template block<numStates, numStates>(numStates, 0).setZero();
    M.template block<numStates, numStates>(numStates, numStates) = contA.transpose();
    M *= dt;
 
    Eigen::Matrix<double, numStates * 2, numStates* 2> G = M.exp();
 
    Eigen::Matrix<double, numStates, numStates> AinvQ =
        G.block(0, numStates, numStates, numStates); // this is discA^-1 * Q
    discA = G.block(numStates, numStates, numStates, numStates).transpose();
 
    Eigen::Matrix<double, numStates, numStates> q = discA * AinvQ; // A * A^-1 * Q = Q
    discQ = (q + q.transpose()) / 2.0; // make Q symmetric again if it became asymmetric
}

template <int numStates>
Eigen::Matrix<double, numStates, numStates>
discretizeQ(const Eigen::Matrix<double, numStates, numStates>& contA,
            const Eigen::Matrix<double, numStates, numStates>& contQ, double dt) {
    Eigen::Matrix<double, numStates, numStates> discA;
    Eigen::Matrix<double, numStates, numStates> discQ;
    discretizeAQ(contA, contQ, discA, discQ, dt);
    return discQ;
}

template <int numStates>
Eigen::Matrix<double, numStates, numStates>
discretizeR(const Eigen::Matrix<double, numStates, numStates>& contR, double dt) {
    return contR / dt;
}

template <int numStates>
static Eigen::Matrix<double, numStates, numStates>
DARE(const Eigen::Matrix<double, numStates, numStates>& A,
     const Eigen::Matrix<double, numStates, numStates>& B,
     const Eigen::Matrix<double, numStates, numStates>& Q,
     const Eigen::Matrix<double, numStates, numStates>& R, double tolerance = 1e-4,
     int maxIter = -1) {
    using mat_t = Eigen::Matrix<double, numStates, numStates>;
    // see:
    // https://scicomp.stackexchange.com/questions/30757/discrete-time-algebraic-riccati-equation-dare-solver-in-c
    // to conform to the wikipedia article conventions, we replace H with P
 
    mat_t currA = A;
    mat_t currG = B * R.inverse() * B.transpose();
    mat_t currP = Q;
 
    mat_t I = mat_t::Identity();
 
    double error;
 
    for (int i = 0; maxIter < 0 || i < maxIter; i++) {
        mat_t lastA = currA;
        mat_t lastG = currG;
        mat_t lastP = currP;
 
        mat_t AIGHinv = lastA * (I + lastG * lastP).inverse();
 
        currA = AIGHinv * lastA;
        currG = lastG + AIGHinv * lastG * lastA.transpose();
        currP = lastP + lastA.transpose() * lastP * (I + lastG * lastP).inverse() * lastA;
 
        double error = (currP - lastP).norm() / currP.norm();
        if (error > tolerance){
            break;
        }
    }
 
    return currP;
}

template <int stateDim, int size, int paramSize> class NoiseCovMat {
public:
    using state_t = Eigen::Matrix<double, stateDim, 1>;
    using param_t = Eigen::Matrix<double, paramSize, 1>;
 
    static_assert(stateDim > 0 && size > 0 && paramSize > 0, "Positive sizes are required!");

    explicit NoiseCovMat(const Eigen::Matrix<double, size, 1>& stdDevs)
        : NoiseCovMat(createCovarianceMatrix(stdDevs)) {}

    NoiseCovMat(const Eigen::Matrix<double, size, size>& mat)
        : func([mat](const state_t& x, const param_t& param) { return mat; }) {}

    NoiseCovMat(const std::function<Eigen::Matrix<double, size, size>(const state_t&,
                                                                      const param_t&)>& func)
        : func(func) {}

    Eigen::Matrix<double, size, size> get(const state_t& x, const param_t& param) {
        return func(x, param);
    }
 
private:
    std::function<Eigen::Matrix<double, size, size>(const state_t&, const param_t&)> func;
};

template <>
class NoiseCovMat<-1, -1, -1> {
public:
    const int stateDim, size, paramDim;

    NoiseCovMat(const Eigen::VectorXd& stdDevs, int stateDim, int paramSize);

    NoiseCovMat(const Eigen::MatrixXd& mat, int stateDim, int paramSize);

    NoiseCovMat(const std::function<Eigen::MatrixXd(const Eigen::VectorXd&,
                                                    const Eigen::VectorXd&)>& func,
                int stateDim, int size, int paramSize);

    Eigen::MatrixXd get(const Eigen::VectorXd& x, const Eigen::VectorXd& param) const;
 
private:
    std::function<Eigen::MatrixXd(const Eigen::VectorXd&, const Eigen::VectorXd&)> func;
};
 
using NoiseCovMatX = NoiseCovMat<-1, -1, -1>;
 
} // namespace filters::statespace