diff --git a/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.cpp b/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.cpp
index 79c608f8107bea2af36334835cd4545b94fda712..318ed6eb747bc0c92f9935050e385854b5df29e2 100644
--- a/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.cpp
+++ b/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.cpp
@@ -25,23 +25,32 @@
* - https://ieeexplore.ieee.org/document/8206066/
*/
-#include <utility>
+#include "UnscentedKalmanFilter.h"
+
#include <complex>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <utility>
-#include "UnscentedKalmanFilter.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-template<typename SystemModelT>
-UnscentedKalmanFilter<SystemModelT>::UnscentedKalmanFilter(const PropCovT &Q, ObsCovT R, const AlphaT &alpha,
- StateT state0, StateCovT P0)
- : Q_(Q), chol_Q_(Q.llt().matrixL().transpose()), R_(std::move(R)), alpha_(alpha), state_(std::move(state0)),
- P_(std::move(P0)), weights_(alpha)
+template <typename SystemModelT>
+UnscentedKalmanFilter<SystemModelT>::UnscentedKalmanFilter(const PropCovT& Q,
+ ObsCovT R,
+ const AlphaT& alpha,
+ StateT state0,
+ StateCovT P0) :
+ Q_(Q),
+ chol_Q_(Q.llt().matrixL().transpose()),
+ R_(std::move(R)),
+ alpha_(alpha),
+ state_(std::move(state0)),
+ P_(std::move(P0)),
+ weights_(alpha)
{
-
}
-template<typename SystemModelT>
-void UnscentedKalmanFilter<SystemModelT>::propagation(const ControlT &omega, FloatT dt)
+template <typename SystemModelT>
+void
+UnscentedKalmanFilter<SystemModelT>::propagation(const ControlT& omega, FloatT dt)
{
StateCovT P = P_ + eps * StateCovT::Identity();
ARMARX_CHECK(P.allFinite());
@@ -51,30 +60,33 @@ void UnscentedKalmanFilter<SystemModelT>::propagation(const ControlT &omega, Flo
// set sigma points (Xi in paper when on Manifold, sigma in Tangent Space)
Eigen::LLT<StateCovT> llt = P.llt();
- if (llt.info() != Eigen::ComputationInfo::Success) {
+ if (llt.info() != Eigen::ComputationInfo::Success)
+ {
P = calculateClosestPosSemidefMatrix(P);
P += eps * StateCovT::Identity();
llt = P.llt();
ARMARX_CHECK(llt.info() == Eigen::ComputationInfo::Success);
}
- StateCovT xi_j_before = weights_.state.sqrt_d_lambda * llt.matrixL().transpose().toDenseMatrix();
+ StateCovT xi_j_before =
+ weights_.state.sqrt_d_lambda * llt.matrixL().transpose().toDenseMatrix();
Eigen::Matrix<FloatT, 2 * dim::state, dim::state> xi_j_after;
Eigen::Matrix<FloatT, 2 * dim::control, dim::state> w_j_after;
- auto propagate_and_retract = [&omega, &dt, &state_after](long row, long offset, const StateT &state,
- const ControlNoiseT &noise,
- auto &xi_j) -> void
+ auto propagate_and_retract = [&omega, &dt, &state_after](long row,
+ long offset,
+ const StateT& state,
+ const ControlNoiseT& noise,
+ auto& xi_j) -> void
{
const StateT sig_j_after = SystemModelT::propagationFunction(state, omega, noise, dt);
xi_j.row(offset + row) = SystemModelT::inverseRetraction(state_after, sig_j_after);
};
- auto calculate_covariance = [](FloatT wj, FloatT w0, auto &xi_j) -> StateCovT
+ auto calculate_covariance = [](FloatT wj, FloatT w0, auto& xi_j) -> StateCovT
{
SigmaPointsT xi_0 = wj * xi_j.colwise().sum();
xi_j.rowwise() -= xi_0.transpose();
- StateCovT cov_after =
- wj * xi_j.transpose() * xi_j + w0 * xi_0 * xi_0.transpose();
+ StateCovT cov_after = wj * xi_j.transpose() * xi_j + w0 * xi_0 * xi_0.transpose();
return cov_after;
};
@@ -102,18 +114,19 @@ void UnscentedKalmanFilter<SystemModelT>::propagation(const ControlT &omega, Flo
StateCovT new_P = P_after + Q;
P_ = std::move((new_P + new_P.transpose()) / 2.0f);
state_ = std::move(state_after);
-
}
-template<typename SystemModelT>
-void UnscentedKalmanFilter<SystemModelT>::update(const ObsT &y)
+template <typename SystemModelT>
+void
+UnscentedKalmanFilter<SystemModelT>::update(const ObsT& y)
{
StateCovT P = P_ + eps * StateCovT::Identity();
ARMARX_CHECK(P.allFinite());
StateCovT xi_j = weights_.state.sqrt_d_lambda * P.llt().matrixL().transpose().toDenseMatrix();
ARMARX_CHECK(P.llt().info() == Eigen::ComputationInfo::Success);
ARMARX_CHECK(xi_j.allFinite());
- Eigen::Matrix<FloatT, 2 * dim::state, dim::obs> y_j = Eigen::Matrix<FloatT, 2 * dim::state, dim::obs>::Zero();
+ Eigen::Matrix<FloatT, 2 * dim::state, dim::obs> y_j =
+ Eigen::Matrix<FloatT, 2 * dim::state, dim::obs>::Zero();
ObsT y_hat = SystemModelT::observationFunction(state_);
for (long j = 0; j < dim::state; j++)
@@ -125,62 +138,68 @@ void UnscentedKalmanFilter<SystemModelT>::update(const ObsT &y)
y_j.row(j + dim::state) = SystemModelT::observationFunction(sig_j_minus);
}
ARMARX_CHECK(y_j.allFinite());
- const ObsT y_mean = weights_.update.wm * y_hat + weights_.update.wj * y_j.colwise().sum().transpose();
+ const ObsT y_mean =
+ weights_.update.wm * y_hat + weights_.update.wj * y_j.colwise().sum().transpose();
y_j.rowwise() -= y_mean.transpose();
y_hat -= y_mean;
- const ObsCovT P_yy = weights_.update.w0 * y_hat * y_hat.transpose() + weights_.update.wj * y_j.transpose() * y_j + R_;
+ const ObsCovT P_yy = weights_.update.w0 * y_hat * y_hat.transpose() +
+ weights_.update.wj * y_j.transpose() * y_j + R_;
ARMARX_CHECK(P_yy.allFinite());
const Eigen::Matrix<FloatT, dim::state, dim::obs> P_xiy =
- weights_.update.wj * (Eigen::Matrix<FloatT, dim::state, 2 * dim::state>() << xi_j, -xi_j).finished() * y_j /* + weights_.update.w0 * xi_0 * y_hat.transpose()*/;
+ weights_.update.wj *
+ (Eigen::Matrix<FloatT, dim::state, 2 * dim::state>() << xi_j, -xi_j).finished() *
+ y_j /* + weights_.update.w0 * xi_0 * y_hat.transpose()*/;
ARMARX_CHECK(P_xiy.allFinite());
- Eigen::Matrix<FloatT, dim::state, dim::obs> K = P_yy.colPivHouseholderQr().solve(P_xiy.transpose()).transpose();
+ Eigen::Matrix<FloatT, dim::state, dim::obs> K =
+ P_yy.colPivHouseholderQr().solve(P_xiy.transpose()).transpose();
FloatT relative_error = (P_yy * K.transpose() - P_xiy.transpose()).norm() / P_xiy.norm();
ARMARX_CHECK(relative_error < 1e-4);
ARMARX_CHECK(K.allFinite());
SigmaPointsT xi_plus = K * (y - y_mean);
ARMARX_CHECK(xi_plus.allFinite());
state_ = SystemModelT::retraction(state_, xi_plus);
- StateCovT new_P = P - K * (P_yy) * K.transpose();
+ StateCovT new_P = P - K * (P_yy)*K.transpose();
// make result symmetric
P_ = calculateClosestPosSemidefMatrix(new_P);
}
-template<typename SystemModelT>
-const typename UnscentedKalmanFilter<SystemModelT>::StateT &UnscentedKalmanFilter<SystemModelT>::getCurrentState() const
+template <typename SystemModelT>
+const typename UnscentedKalmanFilter<SystemModelT>::StateT&
+UnscentedKalmanFilter<SystemModelT>::getCurrentState() const
{
return state_;
}
-template<typename SystemModelT>
-const typename UnscentedKalmanFilter<SystemModelT>::StateCovT &
+template <typename SystemModelT>
+const typename UnscentedKalmanFilter<SystemModelT>::StateCovT&
UnscentedKalmanFilter<SystemModelT>::getCurrentStateCovariance() const
{
return P_;
}
-template<typename SystemModelT>
+template <typename SystemModelT>
typename UnscentedKalmanFilter<SystemModelT>::StateCovT
-UnscentedKalmanFilter<SystemModelT>::calculateClosestPosSemidefMatrix(const UnscentedKalmanFilter::StateCovT &cov)
+UnscentedKalmanFilter<SystemModelT>::calculateClosestPosSemidefMatrix(
+ const UnscentedKalmanFilter::StateCovT& cov)
{
const StateCovT new_P = ((cov + cov.transpose()) / 2.0f);
Eigen::EigenSolver<StateCovT> solver(new_P);
- Eigen::Matrix<FloatT, 6, 6> D = solver.eigenvalues().real().asDiagonal();
- const Eigen::Matrix<FloatT, 6, 6> V = solver.eigenvectors().real();
+ Eigen::Matrix<FloatT, dim::state, dim::state> D = solver.eigenvalues().real().asDiagonal();
+ const Eigen::Matrix<FloatT, dim::state, dim::state> V = solver.eigenvectors().real();
D = D.cwiseMax(0);
return (V * D * V.inverse());
-
}
-template<typename SystemModelT>
-UnscentedKalmanFilter<SystemModelT>::Weights::Weights(AlphaT alpha) : state(dim::state, alpha(0)),
- control(dim::control, alpha(1)),
- update(dim::state, alpha(2))
-{}
+template <typename SystemModelT>
+UnscentedKalmanFilter<SystemModelT>::Weights::Weights(AlphaT alpha) :
+ state(dim::state, alpha(0)), control(dim::control, alpha(1)), update(dim::state, alpha(2))
+{
+}
-template<typename SystemModelT>
+template <typename SystemModelT>
UnscentedKalmanFilter<SystemModelT>::Weights::W::W(size_t l, FloatT alpha)
{
ARMARX_CHECK_GREATER(alpha, 0);
@@ -191,14 +210,10 @@ UnscentedKalmanFilter<SystemModelT>::Weights::W::W(size_t l, FloatT alpha)
w0 = m / (m + l) + 3 - alpha * alpha;
}
-template
-class UnscentedKalmanFilter<SystemModelSE3<float>>;
+template class UnscentedKalmanFilter<SystemModelSE3<float>>;
-template
-class UnscentedKalmanFilter<SystemModelSE3<double>>;
+template class UnscentedKalmanFilter<SystemModelSE3<double>>;
-template
-class UnscentedKalmanFilter<SystemModelSO3xR3<float>>;
+template class UnscentedKalmanFilter<SystemModelSO3xR3<float>>;
-template
-class UnscentedKalmanFilter<SystemModelSO3xR3<double>>;
\ No newline at end of file
+template class UnscentedKalmanFilter<SystemModelSO3xR3<double>>;
diff --git a/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h b/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h
index 0b005f495873de641643e0cc2dda38c43d2ab74f..b7da12e5027bafc5786c01e13e6063a32bc215cc 100644
--- a/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h
+++ b/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h
@@ -29,20 +29,22 @@
#define ROBDEKON_UNSCENTEDKALMANFILTER_H
#include <Eigen/Dense>
+
#include "SystemModel.h"
-template<typename SystemModelT>
+template <typename SystemModelT>
class UnscentedKalmanFilter
{
public:
using FloatT = typename SystemModelT::FloatT;
- static constexpr float eps = 10*std::numeric_limits<float>::epsilon();
+ static constexpr float eps = 10 * std::numeric_limits<float>::epsilon();
// variable dimensions:
using dim = typename SystemModelT::dim;
using StateT = typename SystemModelT::StateT;
using ControlT = typename SystemModelT::ControlT;
- using ControlNoiseT = typename SystemModelT::ControlNoiseT; // todo: correct name? Is it the same as ControlT?
+ using ControlNoiseT =
+ typename SystemModelT::ControlNoiseT; // todo: correct name? Is it the same as ControlT?
using SigmaPointsT = typename SystemModelT::SigmaPointsT; // todo: rename
using StateCovT = Eigen::Matrix<FloatT, dim::state, dim::state>;
@@ -52,8 +54,11 @@ public:
using AlphaT = Eigen::Matrix<FloatT, 3, 1>;
- UnscentedKalmanFilter(const PropCovT& Q, ObsCovT R,
- const AlphaT& alpha, StateT state0, StateCovT P0);
+ UnscentedKalmanFilter(const PropCovT& Q,
+ ObsCovT R,
+ const AlphaT& alpha,
+ StateT state0,
+ StateCovT P0);
UnscentedKalmanFilter() = delete;
private:
@@ -64,8 +69,10 @@ private:
StateT state_;
StateCovT P_;
- struct Weights {
- struct W {
+ struct Weights
+ {
+ struct W
+ {
W(size_t l, FloatT alpha);
float sqrt_d_lambda, wj, wm, w0;
};
@@ -82,7 +89,6 @@ public:
void update(const ObsT& y);
const StateT& getCurrentState() const;
const StateCovT& getCurrentStateCovariance() const;
-
};
extern template class UnscentedKalmanFilter<SystemModelSE3<float>>;