FlexibleKalmanBase.h

Go to the documentation of this file.

// Copyright 2015 Sensics, Inc.
//
// 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 INCLUDED_FlexibleKalmanBase_h_GUID_E4CC84A0_1B4F_4389_95ED_9C37F5FEE95D
#define INCLUDED_FlexibleKalmanBase_h_GUID_E4CC84A0_1B4F_4389_95ED_9C37F5FEE95D

// Internal Includes
#include <osvr/Util/EigenCoreGeometry.h>

// Library/third-party includes
// - none

// Standard includes
#include <type_traits>

#ifndef OSVR_KALMAN_DEBUG_OUTPUT
#define OSVR_KALMAN_DEBUG_OUTPUT(Name, Value)
#endif

namespace osvr {
namespace kalman {
    namespace types {
        using Scalar = double;

        using DimensionType = std::size_t;

        template <DimensionType n>
        using DimensionConstant = std::integral_constant<DimensionType, n>;

        struct HasDimensionBase {};
    } // namespace types

    template <types::DimensionType DIM>
    struct HasDimension : types::HasDimensionBase {
        using Dimension = types::DimensionConstant<DIM>;
    };

    namespace types {
        namespace detail {
            template <typename T, typename = void> struct Dimension_impl {
                using type = DimensionConstant<T::DIMENSION>;
            };
            // explicit specialization
            template <DimensionType n>
            struct Dimension_impl<DimensionConstant<n>, void> {
                using type = DimensionConstant<n>;
            };
            // explicit specialization
            template <typename T>
            struct Dimension_impl<T, typename std::enable_if<std::is_base_of<
                                         HasDimensionBase, T>::value>::type> {
                using type = typename T::Dimension;
            };
        } // namespace detail
        template <typename T>
        using Dimension = typename detail::Dimension_impl<T>::type;

        template <typename FilterType>
        using StateType = typename FilterType::State;

        template <typename FilterType>
        using ProcessModelType = typename FilterType::ProcessModel;

        template <DimensionType n> using Vector = Eigen::Matrix<Scalar, n, 1>;

        template <typename T> using DimVector = Vector<Dimension<T>::value>;

        template <DimensionType n>
        using SquareMatrix = Eigen::Matrix<Scalar, n, n>;

        template <typename T>
        using DimSquareMatrix = SquareMatrix<Dimension<T>::value>;

        template <DimensionType n>
        using DiagonalMatrix = Eigen::DiagonalMatrix<Scalar, n>;

        template <typename T>
        using DimDiagonalMatrix = DiagonalMatrix<Dimension<T>::value>;

        template <DimensionType m, DimensionType n>
        using Matrix = Eigen::Matrix<Scalar, m, n>;

        template <typename T, typename U>
        using DimMatrix = Matrix<Dimension<T>::value, Dimension<U>::value>;

    } // namespace types

    template <typename StateType, typename ProcessModelType>
    inline types::DimSquareMatrix<StateType>
    predictErrorCovariance(StateType const &state,
                           ProcessModelType &processModel, double dt) {
        types::DimSquareMatrix<StateType> A =
            processModel.getStateTransitionMatrix(state, dt);
        // OSVR_KALMAN_DEBUG_OUTPUT("State transition matrix", A);
        types::DimSquareMatrix<StateType> P = state.errorCovariance();
        // auto Q = processModel.getSampledProcessNoiseCovariance(dt);
        OSVR_KALMAN_DEBUG_OUTPUT(
            "Process Noise Covariance Q",
            processModel.getSampledProcessNoiseCovariance(dt));
        return A * P * A.transpose() +
               processModel.getSampledProcessNoiseCovariance(dt);
    }

} // namespace kalman
} // namespace osvr

#endif // INCLUDED_FlexibleKalmanBase_h_GUID_E4CC84A0_1B4F_4389_95ED_9C37F5FEE95D