Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[eigen] API and implementation generalization #81

Merged
merged 1 commit into from
Aug 8, 2022
Merged

[eigen] API and implementation generalization #81

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
52 changes: 27 additions & 25 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,15 +10,15 @@ The library supports simple and extended filters. The update equation uses the J
- [6x2 Constant Acceleration Dynamic Model](#6x2-constant-acceleration-dynamic-model)
- [4x1 Non-Linear Dynamic Model](#4x1-non-linear-dynamic-model)
- [Continuous Integration & Deployment Actions](#continuous-integration--deployment-actions)
- [Motivation](#motivation)
- [Installation](#installation)
- [Class kalman](#class-kalman)
- [Template Parameters](#template-parameters)
- [Member Types](#member-types)
- [Member Functions](#member-functions)
- [Characteristics](#characteristics)
- [Modifiers](#modifiers)
- [Format](#format)
- [Installation](#installation)
- [Motivation](#motivation)
- [Resources](#resources)
- [License](#license)

Expand Down Expand Up @@ -162,24 +162,12 @@ k(drift_x, drift_y, position_x, position_y, variometer);
<br>
[![Deploy Code Coverage: Coveralls](https://github.com/FrancoisCarouge/Kalman/actions/workflows/deploy_test_coverage_coveralls.yml/badge.svg)](https://github.com/FrancoisCarouge/Kalman/actions/workflows/deploy_test_coverage_coveralls.yml)


# Motivation

Kalman filters can be difficult to learn, use, and implement. Users often need fair algebra, domain, and software knowledge. Inadequacy leads to incorrectness, underperformance, and a big ball of mud.

This package explores what could be a Kalman filter implementation a la standard library. The following concerns and tradeoffs are considered:
- Separation of the application domain.
- Separation of the algebra implementation.
- Generalization of the support.

# Installation

See installaton instructions in [INSTALL.txt](INSTALL.txt).

# Class kalman

Defined in header [fcarouge/kalman.hpp](include/fcarouge/kalman.hpp)

A Bayesian filter that uses multivariate Gaussians. Applicable for unimodal and uncorrelated uncertainties. Kalman filters assume white noise, propagation and measurement functions are differentiable, and that the uncertainty stays centered on the state estimate. The filter updates estimates by multiplying Gaussians and predicts estimates by adding Gaussians. Design the state (X, P), the process (F, Q), the measurement (Z, R), the measurement function H, and if the system has control inputs (U, B). Designing a filter is as much art as science. Filters with `state x output x input` dimensions as 1x1x1 and 1x1x0 (no input) are supported through the Standard Templated Library (STL). Higher dimension filters require Eigen 3 support.

```cpp
template <
typename Type,
Expand All @@ -199,15 +187,16 @@ class kalman

| Template Parameter | Definition |
| --- | --- |
| `State` | The type template parameter of the state column vector x. State variables can be observed (measured), or hidden variables (inferred). This is the the mean of the multivariate Gaussian. |
| `Output` | The type template parameter of the measurement column vector z. |
| `Input` | The type template parameter of the control u. A `void` input type can be used for systems with no input control to disable all of the input control features, the control transition matrix G support, and the other related computations from the filter. |
| `Transpose` | The customization point object template parameter of the matrix transpose functor. |
| `Symmetrize` | The customization point object template parameter of the matrix symmetrization functor. |
| `Divide` | The customization point object template parameter of the matrix division functor. |
| `Identity` | The customization point object template parameter of the matrix identity functor. |
| `UpdateTypes` | The additional update function parameter types passed in through a tuple-like parameter type, composing zero or more types. Parameters such as delta times, variances, or linearized values. The parameters are propagated to the function objects used to compute the state observation H and the observation noise R matrices. The parameters are also propagated to the state observation function object h. |
| `PredictionTypes` | The additional prediction function parameter types passed in through a tuple-like parameter type, composing zero or more types. Parameters such as delta times, variances, or linearized values. The parameters are propagated to the function objects used to compute the process noise Q, the state transition F, and the control transition G matrices. The parameters are also propagated to the state transition function object f. |
| `Type` | The type template parameter of the filter data value type used for computation. Defaults to `double`. |
| `State` | The non-type template size of the state column vector x. State variables can be observed (measured), or hidden variables (inferred). This is the the mean of the multivariate Gaussian. Defaults to `1`. |
| `Output` | The non-type template size of the measurement column vector z. Defaults to `1`. |
| `Input` | The non-type template size of the control column vector u. A zero `0` input size value can be used for systems with no input control to disable all of the input control features, the control transition matrix G support, and the other related computations from the filter. Defaults to `0`. |
| `Transpose` | The customization point object template parameter of the matrix transpose functor. Defaults to the standard passthrough `std::identity` function object since the transposed value of an arithmetic type is itself. |
| `Symmetrize` | The customization point object template parameter of the matrix symmetrization functor. Defaults to the standard passthrough `std::identity` function object since the symmetric value of an arithmetic type is itself. |
| `Divide` | The customization point object template parameter of the matrix division functor. Default to the standard division `std::divides<void>` function object. |
| `Identity` | The customization point object template parameter of the matrix identity functor. Defaults to an `identity_matrix` function object returning the arithmetic `1` value. |
| `UpdateTypes` | The additional update function parameter types passed in through a tuple-like parameter type, composing zero or more types. Parameters such as delta times, variances, or linearized values. The parameters are propagated to the function objects used to compute the state observation H and the observation noise R matrices. The parameters are also propagated to the state observation function object h. Defaults to no parameter types, the empty pack. |
| `PredictionTypes` | The additional prediction function parameter types passed in through a tuple-like parameter type, composing zero or more types. Parameters such as delta times, variances, or linearized values. The parameters are propagated to the function objects used to compute the process noise Q, the state transition F, and the control transition G matrices. The parameters are also propagated to the state transition function object f. Defaults to no parameter types, the empty pack. |

## Member Types

Expand Down Expand Up @@ -272,6 +261,19 @@ std::string message{ std::format("{}", k) };
// {f:1,h:1,k:1,p:1,q:0,r:0,s:1,x:0,y:0,z:0}
```

# Installation

See installaton instructions in [INSTALL.txt](INSTALL.txt).

# Motivation

Kalman filters can be difficult to learn, use, and implement. Users often need fair algebra, domain, and software knowledge. Inadequacy leads to incorrectness, underperformance, and a big ball of mud.

This package explores what could be a Kalman filter implementation a la standard library. The following concerns and tradeoffs are considered:
- Separation of the application domain.
- Separation of the algebra implementation.
- Generalization of the support.

# Resources

Awesome resources to learn about Kalman filters:
Expand Down
214 changes: 43 additions & 171 deletions include/fcarouge/internal/eigen.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,11 +39,6 @@ For more information, please refer to <https://unlicense.org> */
#ifndef FCAROUGE_INTERNAL_EIGEN_HPP
#define FCAROUGE_INTERNAL_EIGEN_HPP

//! @file
//! @brief Kalman operation for Eigen 3 types.
//!
//! @details Default customization point objects (CPO).

#include "fcarouge/kalman.hpp"

#include <Eigen/Eigen>
Expand All @@ -56,213 +51,90 @@ For more information, please refer to <https://unlicense.org> */
namespace fcarouge::eigen::internal
{

//! @brief Arithmetic concept.
template <typename Type>
concept arithmetic = std::integral<Type> || std::floating_point<Type>;

//! @brief Function object for performing Eigen matrix transposition.
//!
//! @details Implemented with the Eigen linear algebra library matrices with
//! sizes fixed at compile-time.
struct transpose {
//! @brief Returns the transpose of `value`.
//!
//! @param value Value to compute the transpose of.
//!
//! @exception May throw implementation-defined exceptions.
[[nodiscard]] inline constexpr auto operator()(const auto &value) const
using empty_pack = fcarouge::internal::empty_pack;

struct matrix {
template <typename Type>
[[nodiscard]] inline constexpr auto operator()(const Type &value) ->
typename std::decay_t<Type>::PlainMatrix
{
using type = std::decay_t<decltype(value)>;
using result_type =
typename Eigen::Matrix<typename type::Scalar, type::ColsAtCompileTime,
type::RowsAtCompileTime>;
return value;
}

return result_type{ value.transpose() };
[[nodiscard]] inline constexpr auto operator()(const arithmetic auto &value)
{
using type = std::decay_t<decltype(value)>;
return Eigen::Matrix<type, 1, 1>{ value };
}

//! @brief Returns the transpose of `value`.
//!
//! @param value Value to compute the transpose of.
//!
//! @todo Can this be optimized?
template <typename Type, std::size_t Size>
[[nodiscard]] inline constexpr auto
operator()(const arithmetic auto &value) const
operator()(const std::array<Type, Size> &value)
{
return value;
return Eigen::Matrix<Type, Size, 1>{ value.data() };
}
};

//! @brief Function object for performing Eigen matrix symmetrization.
//!
//! @details Implemented with the Eigen linear algebra library matrices with
//! sizes fixed at compile-time.
struct symmetrize {
//! @brief Returns the symmetrized `value`.
//!
//! @param value Value to compute the symmetry of.
//!
//! @exception May throw implementation-defined exceptions.
[[nodiscard]] inline constexpr auto operator()(const auto &value) const
struct transpose {
template <typename Type>
[[nodiscard]] inline constexpr auto operator()(const Type &value) const ->
typename Eigen::Transpose<Type>::PlainMatrix
{
using result_type = std::decay_t<decltype(value)>;

return result_type{ (value + value.transpose()) / 2 };
return value.transpose();
}
};

//! @brief Returns the symmetrized `value`.
//!
//! @param value Value to compute the symmetry of.
//!
//! @todo Can this be optimized?
[[nodiscard]] inline constexpr auto
operator()(const arithmetic auto &value) const
struct symmetrize {
//! @todo Protect overflow? Is there a better way?
[[nodiscard]] inline constexpr auto operator()(const auto &value) const
{
return value;
return (value + value.transpose()) / 2;
}
};

//! @brief Function object for performing Eigen matrix division.
//!
//! @details Implemented with the Eigen linear algebra library matrices with
//! sizes fixed at compile-time.
struct divide {
//! @brief Returns the quotient of `numerator` and `denominator`.
//!
//! @param numerator The dividend matrix of the division. N: m x n
//! @param denominator The divisor matrix of the division. D: o x n
//!
//! @return The quotient matrix. Q: m x o
//!
//! @exception May throw implementation-defined exceptions.
//!
//! @todo Why compilation fails if we specify the return type in the body of
//! the function?
template <typename Numerator, typename Denominator>
[[nodiscard]] inline constexpr auto
operator()(const Numerator &numerator, const Denominator &denominator) const
-> typename Eigen::Matrix<typename std::decay_t<Numerator>::Scalar,
std::decay_t<Numerator>::RowsAtCompileTime,
std::decay_t<Denominator>::RowsAtCompileTime>
{
return denominator.transpose()
.fullPivHouseholderQr()
.solve(numerator.transpose())
.transpose();
}
// Numerator [m x n] / Denominator [o x n] -> Quotient [m x o]
using result = typename Eigen::Matrix<
typename std::decay_t<std::invoke_result_t<matrix, Numerator>>::Scalar,
std::decay_t<std::invoke_result_t<matrix, Numerator>>::RowsAtCompileTime,
std::decay_t<
std::invoke_result_t<matrix, Denominator>>::RowsAtCompileTime>;

//! @brief Returns the quotient of `numerator` and `denominator`.
//!
//! @param numerator The dividend matrix of the division. N: m x 1
//! @param denominator The divisor value of the division.
//!
//! @return The quotient column vector. Q: m x 1
//!
//! @exception May throw implementation-defined exceptions.
//!
//! @todo Simplify implementation.
template <typename Numerator>
template <typename Numerator, typename Denominator>
[[nodiscard]] inline constexpr auto
operator()(const Numerator &numerator,
const arithmetic auto &denominator) const ->
typename Eigen::Vector<typename std::decay_t<Numerator>::Scalar,
std::decay_t<Numerator>::RowsAtCompileTime>
operator()(const Numerator &numerator, const Denominator &denominator) const
-> result<Numerator, Denominator>
{
return Eigen::Matrix<typename std::decay_t<Numerator>::Scalar, 1, 1>{
denominator
}
matrix to_matrix;
return to_matrix(denominator)
.transpose()
.fullPivHouseholderQr()
.solve(numerator.transpose())
.transpose();
}

//! @brief Returns the quotient of `numerator` and `denominator`.
//!
//! @param numerator The dividend value of the division.
//! @param denominator The divisor matrix of the division. D: o x 1
//!
//! @return The quotient row vector. Q: 1 x o
//!
//! @exception May throw implementation-defined exceptions.
//!
//! @todo Simplify implementation.
template <typename Denominator>
[[nodiscard]] inline constexpr auto
operator()(const arithmetic auto &numerator,
const Denominator &denominator) const ->
typename Eigen::RowVector<typename std::decay_t<Denominator>::Scalar,
std::decay_t<Denominator>::RowsAtCompileTime>
{
return denominator.transpose()
.fullPivHouseholderQr()
.solve(Eigen::Matrix<typename std::decay_t<decltype(numerator)>::Scalar,
1, 1>{ numerator })
.transpose();
}

//! @brief Returns the quotient of `numerator` and `denominator`.
//!
//! @param numerator The dividend value of the division.
//! @param denominator The divisor value of the division.
//!
//! @return The quotient value.
[[nodiscard]] inline constexpr auto
operator()(const arithmetic auto &numerator,
const arithmetic auto &denominator) const
{
return numerator / denominator;
.solve(to_matrix(numerator).transpose())
.transpose()
.eval();
}
};

//! @brief Function object for providing an Eigen identity matrix.
//!
//! @details Implemented with the Eigen linear algebra library matrices with
//! sizes fixed at compile-time.
//!
//! @note Could this function object template be a variable template as proposed
//! @todo Could this function object template be a variable template as proposed
//! in paper P2008R0 entitled "Enabling variable template template parameters"?
struct identity_matrix {
//! @brief Returns the identity matrix.
//!
//! @tparam Type The type template parameter of the matrix.
//!
//! @return The identity matrix `diag(1, 1, ..., 1)`.
//!
//! @exception May throw implementation-defined exceptions.
template <typename Type>
[[nodiscard]] inline constexpr auto operator()() const
[[nodiscard]] inline constexpr auto operator()() const -> Type
{
return Type::Identity();
}

//! @brief Returns `1`, the 1-by-1 identity matrix equivalent.
//!
//! @tparam Type The type template parameter of the value.
//!
//! @return The value `1`.
template <arithmetic Type>
[[nodiscard]] inline constexpr auto operator()() const noexcept
[[nodiscard]] inline constexpr auto operator()() const noexcept -> Type
{
return Type{ 1 };
return 1;
}
};

//! @todo Improve support and optimize for no input: neither type nor void but
//! an equivalent empty type? Void may be more intuitive, practical for the user
//! although less theoretically correct?
template <typename Type = double, std::size_t State = 1, std::size_t Output = 1,
std::size_t Input = 1,
typename UpdateTypes = fcarouge::internal::empty_pack_t,
typename PredictionTypes = fcarouge::internal::empty_pack_t>
using kalman = fcarouge::kalman<
std::conditional_t<State == 1, Type, Eigen::Vector<Type, State>>,
std::conditional_t<Output == 1, Type, Eigen::Vector<Type, Output>>,
std::conditional_t<
Input == 0, void,
std::conditional_t<Input == 1, Type, Eigen::Vector<Type, Input>>>,
transpose, symmetrize, divide, identity_matrix, UpdateTypes,
PredictionTypes>;

} // namespace fcarouge::eigen::internal

#endif // FCAROUGE_INTERNAL_EIGEN_HPP
18 changes: 10 additions & 8 deletions include/fcarouge/internal/format.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,17 +43,19 @@ For more information, please refer to <https://unlicense.org> */

namespace fcarouge
{
template <typename State, typename Output, typename Input, typename Transpose,
typename Symmetrize, typename Divide, typename Identity,
typename UpdateTypes, typename PredictionTypes>
template <typename Type, std::size_t State, std::size_t Output,
std::size_t Input, typename Transpose, typename Symmetrize,
typename Divide, typename Identity, typename UpdateTypes,
typename PredictionTypes>
class kalman;
} // namespace fcarouge

template <typename State, typename Output, typename Input, typename Transpose,
typename Symmetrize, typename Divide, typename Identity,
typename UpdateTypes, typename PredictionTypes, typename Char>
template <typename Type, std::size_t State, std::size_t Output,
std::size_t Input, typename Transpose, typename Symmetrize,
typename Divide, typename Identity, typename UpdateTypes,
typename PredictionTypes, typename Char>
struct std::formatter<
fcarouge::kalman<State, Output, Input, Transpose, Symmetrize, Divide,
fcarouge::kalman<Type, State, Output, Input, Transpose, Symmetrize, Divide,
Identity, UpdateTypes, PredictionTypes>,
Char> {
//! @todo Support parsing arguments.
Expand All @@ -64,7 +66,7 @@ struct std::formatter<

// @todo How to support different nested types?
template <typename OutputIt>
auto format(const fcarouge::kalman<State, Output, Input, Transpose,
auto format(const fcarouge::kalman<Type, State, Output, Input, Transpose,
Symmetrize, Divide, Identity, UpdateTypes,
PredictionTypes> &filter,
std::basic_format_context<OutputIt, Char> &format_context)
Expand Down
Loading