kalman.h

Go to the documentation of this file.

#ifndef FUSED_ALGORITHMS_KALMAN_H
#define FUSED_ALGORITHMS_KALMAN_H
 
#include "config.h"
#include "utilities/expected.h"
#include "matrix_traits.h"
#include "fused/fused_matrix.h"
#include "algorithms/operations/inverse.h"
 
namespace matrix_algorithms
{
 
    using matrix_traits::MatrixStatus;
 
    template <typename T, my_size_t N, my_size_t M>
    Expected<FusedMatrix<T, N, M>, MatrixStatus> kalman_gain(
        const FusedMatrix<T, N, N> &P,
        const FusedMatrix<T, M, N> &H,
        const FusedMatrix<T, M, M> &R)
    {
        static_assert(is_floating_point_v<T>,
                      "kalman_gain requires a floating-point scalar type");
 
        // S = H·P·Hᵀ + R  (M×M)
        auto HP = FusedMatrix<T, M, N>::matmul(H, P);
 
        FusedMatrix<T, M, M> S;
        S = FusedMatrix<T, M, M>::matmul(HP, H.transpose_view()) + R;
 
        // S⁻¹
        auto S_inv_result = inverse(S);
 
        if (!S_inv_result.has_value())
        {
            return Unexpected{S_inv_result.error()};
        }
 
        auto &S_inv = S_inv_result.value();
 
        // K = P·Hᵀ·S⁻¹  (N×M)
        auto PHt = FusedMatrix<T, N, M>::matmul(P, H.transpose_view());
        auto K = FusedMatrix<T, N, M>::matmul(PHt, S_inv);
 
        return move(K);
    }
 
    template <typename T, my_size_t N, my_size_t M>
    FusedMatrix<T, N, N> joseph_update(
        const FusedMatrix<T, N, M> &K,
        const FusedMatrix<T, M, N> &H,
        const FusedMatrix<T, N, N> &P,
        const FusedMatrix<T, M, M> &R)
    {
        static_assert(is_floating_point_v<T>,
                      "joseph_update requires a floating-point scalar type");
 
        // IKH = I - K·H  (N×N)
        FusedMatrix<T, N, N> I;
        I.setIdentity();
 
        auto KH = FusedMatrix<T, N, N>::matmul(K, H);
        FusedMatrix<T, N, N> IKH;
        IKH = I - KH;
 
        // (I-K·H)·P·(I-K·H)ᵀ
        auto tmp = FusedMatrix<T, N, N>::matmul(IKH, P);
        auto term1 = FusedMatrix<T, N, N>::matmul(tmp, IKH.transpose_view());
 
        // K·R·Kᵀ
        auto KR = FusedMatrix<T, N, M>::matmul(K, R);
        auto term2 = FusedMatrix<T, N, N>::matmul(KR, K.transpose_view());
 
        FusedMatrix<T, N, N> result(T(0));
        result = term1 + term2;
        return result;
    }
 
} // namespace matrix_algorithms
 
#endif // FUSED_ALGORITHMS_KALMAN_H