Skip to content
Snippets Groups Projects
Commit bc5ba2b7 authored by Corvin-N's avatar Corvin-N
Browse files

Format files

parent f2e36ee0
No related branches found
No related tags found
1 merge request!279Fix ukfm static dimension
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.cpp
+ 64
− 49
View file @ bc5ba2b7
......@@ -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,44 +138,51 @@ 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);
......@@ -171,16 +191,15 @@ UnscentedKalmanFilter<SystemModelT>::calculateClosestPosSemidefMatrix(const Unsc
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>>;
source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h
+ 14
− 8
View file @ bc5ba2b7
......@@ -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>>;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment