Does UKF have smoothing?
Opened this issue · 0 comments
tugrul512bit commented
I tried gps_position smoothing from one of examples by adding random noise to measurement but it does not smooth it:
#define UKF_DOUBLE_PRECISION
//#define real_t double
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <cmath>
#include "UKF/Types.h"
#include "UKF/Integrator.h"
#include "UKF/StateVector.h"
#include "UKF/MeasurementVector.h"
#include "UKF/Core.h"
/* Set up state vector class. */
enum MyStateFields {
Position,
Velocity,
Attitude,
AngularVelocity
};
using MyStateVector = UKF::StateVector<
UKF::Field<Position, UKF::Vector<3>>,
UKF::Field<Velocity, UKF::Vector<3>>,
UKF::Field<Attitude, UKF::Quaternion>,
UKF::Field<AngularVelocity, UKF::Vector<3>>
>;
namespace UKF {
namespace Parameters {
template <> constexpr real_t AlphaSquared<MyStateVector> = 1e-6;
}
/*
State vector process model. One version takes body frame kinematic
acceleration and angular acceleration as inputs, the other doesn't (assumes
zero accelerations).
*/
template <> template <>
MyStateVector MyStateVector::derivative<UKF::Vector<3>, UKF::Vector<3>>(
const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) const {
MyStateVector temp;
/* Position derivative. */
temp.set_field<Position>(get_field<Velocity>());
/* Velocity derivative. */
temp.set_field<Velocity>(get_field<Attitude>().conjugate() * acceleration);
/* Attitude derivative. */
UKF::Quaternion temp_q;
temp_q.vec() = get_field<AngularVelocity>();
temp_q.w() = 0;
temp.set_field<Attitude>(temp_q);
/* Angular velocity derivative. */
temp.set_field<AngularVelocity>(angular_acceleration);
return temp;
}
template <> template <>
MyStateVector MyStateVector::derivative<>() const {
return derivative(UKF::Vector<3>(0, 0, 0), UKF::Vector<3>(0, 0, 0));
}
}
/* Set up measurement vector class. */
enum MyMeasurementFields {
GPS_Position,
GPS_Velocity,
Accelerometer,
Magnetometer,
Gyroscope
};
using MyMeasurementVector = UKF::DynamicMeasurementVector<
UKF::Field<GPS_Position, UKF::Vector<3>>,
UKF::Field<GPS_Velocity, UKF::Vector<3>>,
UKF::Field<Accelerometer, UKF::Vector<3>>,
UKF::Field<Magnetometer, UKF::FieldVector>,
UKF::Field<Gyroscope, UKF::Vector<3>>
>;
using MyCore = UKF::Core<
MyStateVector,
MyMeasurementVector,
UKF::IntegratorRK4
>;
namespace UKF {
/*
Define measurement model to be used in tests. NOTE: These are just for
testing, don't expect them to make any physical sense whatsoever.
*/
template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, GPS_Position>(const MyStateVector& state) {
return state.get_field<Position>();
}
template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, GPS_Velocity>(const MyStateVector& state) {
return state.get_field<Velocity>();
}
template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, Accelerometer>(const MyStateVector& state) {
return state.get_field<Attitude>() * UKF::Vector<3>(0, 0, -9.8);
}
template <> template <>
UKF::FieldVector MyMeasurementVector::expected_measurement
<MyStateVector, Magnetometer>(const MyStateVector& state) {
return state.get_field<Attitude>() * UKF::FieldVector(1, 0, 0);
}
template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, Gyroscope>(const MyStateVector& state) {
return state.get_field<AngularVelocity>();
}
/*
These versions of the predicted measurement functions take kinematic
acceleration and angular acceleration as inputs. Note that in reality, the
inputs would probably be a control vector and the accelerations would be
calculated using the state vector and a dynamics model.
*/
template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, GPS_Position>(const MyStateVector& state,
const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) {
return state.get_field<Position>();
}
template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, GPS_Velocity>(const MyStateVector& state,
const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) {
return state.get_field<Velocity>();
}
template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, Accelerometer, UKF::Vector<3>>(const MyStateVector& state,
const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) {
return state.get_field<Attitude>() * UKF::Vector<3>(0, 0, -9.8) + acceleration;
}
template <> template <>
UKF::FieldVector MyMeasurementVector::expected_measurement
<MyStateVector, Magnetometer, UKF::Vector<3>>(const MyStateVector& state,
const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) {
return state.get_field<Attitude>() * UKF::FieldVector(1, 0, 0);
}
template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, Gyroscope, UKF::Vector<3>>(const MyStateVector& state,
const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) {
return state.get_field<AngularVelocity>();
}
}
MyCore create_initialised_test_filter() {
MyCore test_filter;
test_filter.state.set_field<Position>(UKF::Vector<3>(0, 0, 0));
test_filter.state.set_field<Velocity>(UKF::Vector<3>(0, 0, 0));
test_filter.state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
test_filter.state.set_field<AngularVelocity>(UKF::Vector<3>(0, 0, 0));
test_filter.covariance = MyStateVector::CovarianceMatrix::Zero();
test_filter.covariance.diagonal() << 10000, 10000, 10000, 100, 100, 100, 1, 1, 5, 10, 10, 10;
test_filter.measurement_covariance << 10, 10, 10, 1, 1, 1, 5e-1, 5e-1, 5e-1, 5e-1, 5e-1, 5e-1, 0.05, 0.05, 0.05;
real_t a, b;
real_t dt = 0.01;
a = std::sqrt(0.1 * dt * dt);
b = std::sqrt(0.1 * dt);
test_filter.process_noise_covariance <<
a, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, a, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, a, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, b, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, b, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, b, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, a, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, a, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, a, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, b, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, b, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, b;
return test_filter;
}
#include <random>
static std::random_device rd; // random device engine, usually based on /dev/random on UNIX-like systems
// initialize Mersennes' twister using rd to generate the seed
static std::mt19937 rng{ rd() };
double dice()
{
static std::uniform_real_distribution<double> uid(-5, 5); // random dice
return uid(rng); // use rng as a generator
}
int main() {
MyCore test_filter = create_initialised_test_filter();
MyMeasurementVector m;
m.set_field<GPS_Position>(UKF::Vector<3>(100, 10, -50));
m.set_field<GPS_Velocity>(UKF::Vector<3>(20, 0, 0));
m.set_field<Accelerometer>(UKF::Vector<3>(0, 0, -14.8));
m.set_field<Magnetometer>(UKF::FieldVector(0, -1, 0));
m.set_field<Gyroscope>(UKF::Vector<3>(0.5, 0, 0));
for (int i = 0; i < 100; i++)
{
// gps position: 100 +/- 5, 10, -50 measured
m.set_field<GPS_Position>(UKF::Vector<3>(100+dice(), 10, -50));
test_filter.step(0.01, m, UKF::Vector<3>(0, 0, -5), UKF::Vector<3>(1, 0, 0));
// does not smooth
std::cout << m.get_field<Position>() << std::endl;
std::cout << "-------------" << std::endl;
}
return 0;
}