UnscentedKalmanFilterTest.cpp File Reference

#include "UnscentedKalmanFilter.h"
#include <cstdlib>
#include <ctime>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/time.h>
#include <sciplot/sciplot.hpp>

Typedefs
using	Matrix = Eigen::Matrix<T, 3, 3>
using	SystemModelT = SystemModelSE3<T>
using	T = float
using	Vector = Eigen::Matrix<T, 3, 1>

Functions
int	main ()
void	simulate_observation (const std::vector< SystemModelT::StateT > &states, std::vector< SystemModelT::ObsT > &observations)
void	simulate_trajectory (std::vector< SystemModelT::StateT > &states, std::vector< SystemModelT::ControlT > &omegas)
void	test_retract ()
void	test_se3 ()

Variables
constexpr T	acc_noise_std = 0.01
constexpr T	c = (1 / max_time) * 2 * M_PI
constexpr T	dt = max_time / num_timesteps
const Vector	initial_offet_pos = 0.5 * Vector(1, 0.5, 0.7)
constexpr T	initial_offset_angle = 0.0 * 10 * M_PI / 180
constexpr T	max_time = 1
constexpr long	num_timesteps = 3000
constexpr T	obs_noise_std = 0.01
constexpr T	om_noise_std = 0.01

Typedef Documentation

Matrix

using Matrix = Eigen::Matrix<T, 3, 3>

SystemModelT

using SystemModelT = SystemModelSE3<T>

T

using T = float

Vector

using Vector = Eigen::Matrix<T, 3, 1>

Function Documentation

main()

int main

(

)

simulate_observation()

void simulate_observation	(	const std::vector< SystemModelT::StateT > &	states,
		std::vector< SystemModelT::ObsT > &	observations )

simulate_trajectory()

void simulate_trajectory	(	std::vector< SystemModelT::StateT > &	states,
		std::vector< SystemModelT::ControlT > &	omegas )

test_retract()

void test_retract

(

)

test_se3()

void test_se3

(

)

Variable Documentation

acc_noise_std

T acc_noise_std = 0.01

constexpr

c

T c = (1 / max_time) * 2 * M_PI

constexpr

dt

T dt = max_time / num_timesteps

constexpr

initial_offet_pos

const Vector initial_offet_pos = 0.5 * Vector(1, 0.5, 0.7)

initial_offset_angle

T initial_offset_angle = 0.0 * 10 * M_PI / 180

constexpr

max_time

T max_time = 1

constexpr

num_timesteps

long num_timesteps = 3000

constexpr

obs_noise_std

T obs_noise_std = 0.01

constexpr

om_noise_std

T om_noise_std = 0.01

constexpr