UnscentedKalmanFilterTest.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    ROBDEKON
 * @author     Christoph Pohl ( christoph dot pohl at kit dot edu )
 * @date       18.03.21
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 *
 * Code adapted to C++ from: https://github.com/CAOR-MINES-ParisTech/ukfm
 * See also:
 *      - https://arxiv.org/pdf/2002.00878.pdf
 *      - https://ieeexplore.ieee.org/document/8206066/
 */
#include "UnscentedKalmanFilter.h"
 
#include <cstdlib> /* srand, rand */
#include <ctime> /* time */
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/time.h>
 
#include <sciplot/sciplot.hpp>
 
using T = float;
using Vector = Eigen::Matrix<T, 3, 1>;
using Matrix = Eigen::Matrix<T, 3, 3>;
using SystemModelT = SystemModelSE3<T>;
 
constexpr long num_timesteps = 3000;
constexpr T max_time = 1;
constexpr T dt = max_time / num_timesteps;
constexpr T c = (1 / max_time) * 2 * M_PI;
 
constexpr T acc_noise_std = 0.01;
constexpr T om_noise_std = 0.01;
constexpr T obs_noise_std = 0.01;
constexpr T initial_offset_angle = 0.0 * 10 * M_PI / 180;
const Vector initial_offet_pos = 0.5 * Vector(1, 0.5, 0.7);
 
void
test_retract()
{
    for (int i = 0; i < num_timesteps; i++)
    {
        SystemModelT::StateT state1, state2;
        state1.pose = manif::SE3<T>::Random();
        //        state1.velocity.setRandom();
        state2.pose = manif::SE3<T>::Random();
        //        state2.velocity.setRandom();
        // sigma = state2 - state1
        SystemModelT::SigmaPointsT sigma = SystemModelT::inverseRetraction(state1, state2);
        // state3 = state1 + sigma --> state3 = state2
        SystemModelT::StateT state3 = SystemModelT::retraction(state1, sigma);
        //        ARMARX_CHECK((state2.velocity - state3.velocity).norm() < 10 * std::numeric_limits<T>::epsilon());
        ARMARX_CHECK((state2.pose - state3.pose).coeffs().norm() <
                     10000 * std::numeric_limits<T>::epsilon())
            << (state2.pose - state3.pose).coeffs().norm();
 
        SystemModelT::SigmaPointsT sigma2 = SystemModelT::inverseRetraction(state1, state3);
        // TODO: fix bad accuracy
        ARMARX_CHECK((sigma2 - sigma).norm() < 10000 * std::numeric_limits<T>::epsilon())
            << (sigma2 - sigma).norm();
    }
}
 
void
test_se3()
{
    for (int i = 1; i < num_timesteps; i++)
    {
        const T t = dt * i;
        const T angle = t * c;
        const Eigen::Matrix<T, 3, 1> pos(std::sin(angle), std::cos(angle), 0);
        manif::SO3<T> rot = manif::SO3<T>(0, 0, angle);
        ARMARX_CHECK(std::abs(rot.log().coeffs()(2) - angle) < 1e-6);
        manif::SE3<T> pose(pos, rot);
 
        manif::SE3Tangent<T> tangent = pose.log();
        ARMARX_CHECK((tangent.coeffs().segment(0, 3) - pos).norm() < 1e-6);
        ARMARX_CHECK(std::abs(tangent.coeffs()(5) - angle) < 1e-6);
    }
}
 
void
simulate_trajectory(std::vector<SystemModelT::StateT>& states,
                    std::vector<SystemModelT::ControlT>& omegas)
{
    SystemModelT::StateT state;
    //    state.velocity = Vector(c, 0, 0);
    state.pose = manif::SE3<T>(0, 1, 0, M_PI, M_PI, 0);
    states.push_back(state);
    for (int i = 1; i < num_timesteps; i++)
    {
        const T t = dt * i;
        const T t_prev = dt * (i - 1);
        const T angle = t * c;
        // yaw goes from -pi to pi
        const T yaw = 1 * M_PI * std::sin(angle);
        const T roll = 0.1 * M_PI * std::sin(angle) + M_PI;
        const T pitch = 0.1 * M_PI * std::sin(angle) + M_PI;
        const Vector pos(std::sin(angle), std::cos(angle), 0);
        manif::SE3Tangent<T> tangent(states.at(i - 1).pose.log());
        ARMARX_CHECK((states.at(i - 1).pose.translation() - pos).norm() < 0.1);
        //        ARMARX_CHECK((tangent.coeffs().segment(0, 3) - pos).norm() < 0.1);
        const Vector vel(std::cos(angle) * c, -std::sin(angle) * c, 0);
        const Vector acc(-std::sin(angle) * (c * c), -std::cos(angle) * (c * c), 0);
        const Vector acc_prev(-std::sin(t_prev * c) * (c * c), -std::cos(t_prev * c) * (c * c), 0);
        //        const Eigen::Vector3f vel((pos - state.position) / dt);
        //        Eigen::Vector3f acc = (vel - state.velocity) / dt;
        manif::SO3<T> rot = manif::SO3<T>(roll, pitch, yaw);
        //        Eigen::Matrix<T, 3, 1> rot_tan = rot.log().coeffs();
        //        ARMARX_CHECK(std::abs(rot_tan(2) - yaw) < 1e-6);
        SystemModelT::ControlT control;
        //        control.linear_accel = state.pose.rotation().inverse() * acc_prev;
        Vector angular_velocity(0.1 * M_PI * std::cos(angle) * c,
                                0.1 * M_PI * std::cos(angle) * c,
                                M_PI * std::cos(angle) * c);
        control.velocity.coeffs() << state.pose.asSO3().inverse().act(vel), angular_velocity;
 
        const SystemModelT::StateT propagated = SystemModelT::propagationFunction(
            state, control, SystemModelT::ControlNoiseT::Zero(), dt);
        manif::SE3Tangent<T> pro_tangent = propagated.pose.log();
        T pos_diff1 = (propagated.pose.translation() - pos).norm();
        //        ARMARX_CHECK(pos_diff1 < pos_diff2);
        ARMARX_CHECK(pos_diff1 < 2e-5)
            << "Position is not the same: " << pos << " vs " << propagated.pose.translation();
        //        ARMARX_CHECK((propagated.velocity - vel).norm() < 1e-4) << "Velocity is not the same: " << vel << " vs " << propagated.velocity;
        //        ARMARX_CHECK(propagated.pose.asSO3().lminus(rot).coeffs().norm() < 1e-2) << "Rotation is not the same: " << rot.rotation() << " vs " << propagated.pose.rotation();
        //        state.velocity = vel;
        state.pose = manif::SE3<T>(pos, rot);
        states.push_back(propagated);
 
        tangent = state.pose.log();
        ARMARX_CHECK((state.pose.translation() - pos).norm() < 1e-6);
        //        ARMARX_CHECK(std::abs(tangent.coeffs()(5) - yaw) < 1e-2);
 
        // add noise
        control.velocity.coeffs().segment(0, 3) += acc_noise_std * Vector::Random();
        control.velocity.coeffs().segment(3, 3) += om_noise_std * Vector::Random();
        omegas.push_back(control);
    }
}
 
void
simulate_observation(const std::vector<SystemModelT::StateT>& states,
                     std::vector<SystemModelT::ObsT>& observations)
{
    for (const auto& state : states)
    {
        SystemModelT::ObsT obs = SystemModelT::observationFunction(state);
        SystemModelT::ObsT true_obs;
        true_obs = state.pose.log().coeffs();
        //        true_obs.segment(0, 3) = state.pose.translation();
        //        true_obs.segment(3, 3) = state.pose.asSO3().log().coeffs();
        ARMARX_CHECK((obs - true_obs).norm() < std::numeric_limits<T>::epsilon());
        observations.push_back(obs + obs_noise_std * SystemModelT::ObsT::Random());
    }
}
 
int
main()
{
    srand(time(NULL));
 
    test_retract();
    //    test_se3();
 
    std::vector<SystemModelT::StateT> states;
    std::vector<SystemModelT::ControlT> omegas;
    std::vector<SystemModelT::ObsT> observations;
    simulate_trajectory(states, omegas);
    simulate_observation(states, observations);
 
    ARMARX_INFO << "Num States: " << states.size() << " Num Controls: " << omegas.size()
                << " Num Obs: " << observations.size();
    UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
        UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
    Q.block<3, 3>(0, 0) *= acc_noise_std * acc_noise_std;
    Q.block<3, 3>(3, 3) *= om_noise_std * om_noise_std;
 
    UnscentedKalmanFilter<SystemModelT>::ObsCovT R =
        UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity() * obs_noise_std * obs_noise_std;
    UnscentedKalmanFilter<SystemModelT>::StateCovT P0 =
        UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
    P0.block<3, 3>(3, 3) *= initial_offset_angle * initial_offset_angle;
    P0.block<3, 3>(0, 0) *= initial_offet_pos.norm() * initial_offet_pos.norm();
    //    P0.block<3, 3>(6, 6) *= 0.0;
    UnscentedKalmanFilter<SystemModelT>::AlphaT alpha =
        UnscentedKalmanFilter<SystemModelT>::AlphaT::Ones() * 1e-3;
 
    SystemModelT::StateT state0;
    manif::SO3<T> rot(0, 0, initial_offset_angle);
    state0.pose = states.at(0).pose.lplus(manif::SE3<T>(initial_offet_pos, rot).log());
    //    state0.velocity = states.at(0).velocity;
    UnscentedKalmanFilter<SystemModelT> ukf(Q, R, alpha, state0, P0);
 
    std::vector<SystemModelT::StateT> ukf_states;
    std::vector<UnscentedKalmanFilter<SystemModelT>::StateCovT> ukf_Ps;
    ukf_states.push_back(state0);
    ukf_Ps.push_back(P0);
 
    std::vector<T> x_true, y_true, z_true, x_obs, y_obs, z_obs, x_ukf, y_ukf, z_ukf;
    std::vector<T> vx_true, vy_true, vz_true, vx_ukf, vy_ukf, vz_ukf;
    std::vector<T> a_true, b_true, c_true, a_obs, b_obs, c_obs, a_ukf, b_ukf, c_ukf;
 
    for (int i = 1; i < num_timesteps; i++)
    {
        // propagate
        TIMING_START(LOOP);
        TIMING_START(PROPAGATION);
        ukf.propagation(omegas.at(i - 1), dt);
        TIMING_END(PROPAGATION);
        if ((i - 1) % 100 == 0)
        {
            TIMING_START(UPDATE);
            ukf.update(observations.at(i));
            TIMING_END(UPDATE);
            TIMING_START(REST);
            const SystemModelT::StateT& current_state = ukf.getCurrentState();
            //            ARMARX_INFO << "Velocity Diff: " << (states.at(i).velocity - current_state.velocity).norm();
            ARMARX_INFO << "Pose Diff: "
                        << (states.at(i).pose - current_state.pose).coeffs().norm();
            //            ARMARX_INFO << "Vel: " << current_state.velocity - states.at(i).velocity;
            ARMARX_INFO << "Max Cov " << ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff();
            ARMARX_INFO
                << "Diag: "
                << (ukf.getCurrentStateCovariance() - Eigen::Matrix<T, 6, 6>::Identity()).norm();
            //            ARMARX_CHECK((states.at(i).position - current_state.position).norm() < 1);
 
            x_true.push_back(states.at(i).pose.log().coeffs()(0));
            y_true.push_back(states.at(i).pose.log().coeffs()(1));
            z_true.push_back(states.at(i).pose.log().coeffs()(2));
 
            x_obs.push_back(observations.at(i)(0));
            y_obs.push_back(observations.at(i)(1));
            z_obs.push_back(observations.at(i)(2));
 
            x_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(0));
            y_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(1));
            z_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(2));
 
            //            vx_true.push_back(states.at(i).velocity(0, 0));
            //            vy_true.push_back(states.at(i).velocity(1, 0));
            //            vz_true.push_back(states.at(i).velocity(2, 0));
            //
            //            vx_ukf.push_back(ukf.getCurrentState().velocity(0, 0));
            //            vy_ukf.push_back(ukf.getCurrentState().velocity(1, 0));
            //            vz_ukf.push_back(ukf.getCurrentState().velocity(2, 0));
 
            a_true.push_back(states.at(i).pose.log().coeffs()(3));
            b_true.push_back(states.at(i).pose.log().coeffs()(4));
            c_true.push_back(states.at(i).pose.log().coeffs()(5));
 
            a_obs.push_back(observations.at(i)(3, 0));
            b_obs.push_back(observations.at(i)(4, 0));
            c_obs.push_back(observations.at(i)(5, 0));
 
            a_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(3));
            b_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(4));
            c_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(5));
            TIMING_END(REST);
        }
 
        ukf_states.push_back(ukf.getCurrentState());
        ukf_Ps.push_back(ukf.getCurrentStateCovariance());
 
        TIMING_END(LOOP);
    }
 
    sciplot::Plot3D position_plot;
    sciplot::Plot pos_plot;
 
    position_plot.drawCurve(x_true, y_true, z_true).label("True").lineWidth(1);
    position_plot.drawCurve(x_obs, y_obs, z_obs).label("Obs").lineWidth(1);
    position_plot.drawCurve(x_ukf, y_ukf, z_ukf).label("UKF").lineWidth(1);
 
    pos_plot.drawCurve(x_true, y_true).label("True");
    pos_plot.drawCurve(x_obs, y_obs).label("Obs");
    pos_plot.drawCurve(x_ukf, y_ukf).label("UKF");
 
    //    sciplot::Plot3D vel_plot;
    //
    //    vel_plot.drawCurve(vx_true, vy_true, vz_true).label("True").lineWidth(1);
    //    vel_plot.drawCurve(vx_ukf, vy_ukf, vz_ukf).label("UKF").lineWidth(1);
 
 
    sciplot::Plot3D orientation_plot;
 
    orientation_plot.drawCurve(a_true, b_true, c_true).label("True").lineWidth(1);
    orientation_plot.drawCurve(a_obs, b_obs, c_obs).label("Obs").lineWidth(1);
    orientation_plot.drawCurve(a_ukf, b_ukf, c_ukf).label("UKF").lineWidth(1);
 
    pos_plot.show();
    position_plot.show();
    //    vel_plot.show();
    orientation_plot.show();
}