Skip to content

Instantly share code, notes, and snippets.

@sunsided

sunsided/OrientationFusion.cpp

Last active August 29, 2015 14:16
Show Gist options
  • Save sunsided/1363cf224718361a38b7 to your computer and use it in GitHub Desktop.
Save sunsided/1363cf224718361a38b7 to your computer and use it in GitHub Desktop.
Download ZIP
Odometry Kalman filter snapshot from a robot control project
Raw
OrientationFusion.cpp
#include "OrientationFusion.h"
#include "time/SystemTime.h"
using namespace Sensors;
#define matrix_set(matrix, row, column, value) \
matrix->data[row][column] = value
#define matrix_set_symmetric(matrix, row, column, value) \
matrix->data[row][column] = value; \
matrix->data[column][row] = value
#define matrix_get(matrix, row, column) \
matrix->data[row][column]
#ifndef FIXMATRIX_MAX_SIZE
#error FIXMATRIX_MAX_SIZE must be defined and greater or equal to the number of states, inputs and measurements.
#endif
OrientationFusion::OrientationFusion()
{
#if (FIXMATRIX_MAX_SIZE < States) || (FIXMATRIX_MAX_SIZE < Inputs) || (FIXMATRIX_MAX_SIZE < Measurements)
#error FIXMATRIX_MAX_SIZE must be greater or equal to the number of states, inputs and measurements.
#endif
/************************************************************************/
/* initialize the filter structures */
/************************************************************************/
kalman_filter_initialize_uc(&kf, States);
kalman_observation_initialize(&kfm, States, Measurements);
/************************************************************************/
/* set initial state */
/************************************************************************/
mf16 *x = kalman_get_state_vector_uc(&kf);
x->data[0][0] = 0; // theta
x->data[1][0] = 0; // omega
x->data[2][0] = 0; // alpha
/************************************************************************/
/* set covariance */
/************************************************************************/
mf16 *P = kalman_get_system_covariance_uc(&kf);
matrix_set_symmetric(P, 0, 0, F16(.029792 * .029792)); // var(v)
matrix_set_symmetric(P, 0, 1, 0); // cov(v,a)
matrix_set_symmetric(P, 0, 2, 0); // cov(v,j)
matrix_set_symmetric(P, 1, 1, F16(.00857434 * .00857434)); // var(a)
matrix_set_symmetric(P, 1, 2, 0); // cov(a,j)
matrix_set_symmetric(P, 2, 2, F16(.0085743 * .0085743)); // var(j)
/************************************************************************/
/* system process noise */
/************************************************************************/
mf16 *Q = kalman_get_system_process_noise_uc(&kf);
matrix_set_symmetric(Q, 0, 0, F16(.029792 * .029792)); // var(v)
matrix_set_symmetric(Q, 0, 1, 0); // cov(v,a)
matrix_set_symmetric(Q, 0, 2, 0); // cov(v,j)
matrix_set_symmetric(Q, 1, 1, F16(.129792 * .129792)); // var(a)
matrix_set_symmetric(Q, 1, 2, 0); // cov(a,j)
matrix_set_symmetric(Q, 2, 2, F16(.0085743 * .0085743)); // var(j)
/************************************************************************/
/* set measurement transformation */
/************************************************************************/
mf16 *H = kalman_get_observation_transformation(&kfm);
matrix_set(H, 0, 0, fix16_one); // z = 1*theta
matrix_set(H, 0, 1, 0); // + 0*omega
matrix_set(H, 0, 2, 0); // + 0*alpha
matrix_set(H, 1, 0, 0); // z = 0*theta
matrix_set(H, 1, 1, fix16_one); // + 1*omega
matrix_set(H, 1, 2, 0); // + 0*alpha
/************************************************************************/
/* set process noise */
/************************************************************************/
mf16 *R = kalman_get_observation_process_noise(&kfm);
matrix_set(R, 0, 0, F16(3*0.129792*0.129792)); // var(theta)
matrix_set(R, 1, 1, F16(0.0085743*0.0085743)); // var(omega)
}
void OrientationFusion::SetStateTransition(milliseconds_t time)
{
mf16 *A = kalman_get_state_transition_uc(&kf);
// set time constant
const fix16_t T = fix16_div(fix16_from_int(time.value), fix16_from_int(1000));
const fix16_t Tsquare = fix16_sq(T);
// helper
const fix16_t fix16_half = fix16_from_float(0.5);
// transition of x to v
matrix_set(A, 0, 0, fix16_one); // 1
matrix_set(A, 0, 1, T); // T
matrix_set(A, 0, 2, fix16_mul(fix16_half, Tsquare)); // 0.5 * T^2
// transition of x to a
matrix_set(A, 1, 0, 0); // 0
matrix_set(A, 1, 1, fix16_one); // 1
matrix_set(A, 1, 2, T); // T
// transition of x to j
matrix_set(A, 2, 0, 0); // 0
matrix_set(A, 2, 1, 0); // 0
matrix_set(A, 2, 2, fix16_one); // 1
}
milliseconds_t OrientationFusion::Predict(Fix16 u_omega)
{
const auto now = SystemTime::get()->NowMs();
const auto difference = (now - _previousRun);
_previousRun = now;
SetStateTransition(difference);
kalman_predict_uc(&kf);
return difference;
}
void OrientationFusion::Update(milliseconds_t dt, Fix16 z_heading, Fix16 z_odometry)
{
/* update */
mf16 *z = kalman_get_observation_vector(&kfm);
matrix_set(z, 0, 0, z_heading);
matrix_set(z, 1, 0, z_odometry);
/* correct */
kalman_correct_uc(&kf, &kfm);
mf16 *P = kalman_get_system_covariance_uc(&kf);
matrix_set_symmetric(P, 0, 1, 0);
matrix_set_symmetric(P, 0, 2, 0);
matrix_set_symmetric(P, 1, 2, 0);
if (kf.x.data[1][0] == 0xFFFFFFFF80000000 || kf.x.data[1][0] == 0x80000000)
{
kf.x.data[0][0] = 0;
kf.P.data[0][0] = Fix16::OneThousand;
kf.x.data[1][0] = 0;
kf.P.data[1][1] = Fix16::OneThousand;
kf.x.data[2][0] = 0;
kf.P.data[2][2] = Fix16::OneThousand;
}
}
Raw
VelocityFusion.cpp
#include "VelocityFusion.h"
#include "time/SystemTime.h"
using namespace Sensors;
#define matrix_set(matrix, row, column, value) \
matrix->data[row][column] = value
#define matrix_set_symmetric(matrix, row, column, value) \
matrix->data[row][column] = value; \
matrix->data[column][row] = value
#define matrix_get(matrix, row, column) \
matrix->data[row][column]
#ifndef FIXMATRIX_MAX_SIZE
#error FIXMATRIX_MAX_SIZE must be defined and greater or equal to the number of states, inputs and measurements.
#endif
VelocityFusion::VelocityFusion()
{
#if (FIXMATRIX_MAX_SIZE < States) || (FIXMATRIX_MAX_SIZE < Inputs) || (FIXMATRIX_MAX_SIZE < Measurements)
#error FIXMATRIX_MAX_SIZE must be greater or equal to the number of states, inputs and measurements.
#endif
/************************************************************************/
/* initialize the filter structures */
/************************************************************************/
kalman_filter_initialize(&kf, States, Inputs);
kalman_observation_initialize(&kfm, States, Measurements);
/************************************************************************/
/* set initial state */
/************************************************************************/
mf16 *x = kalman_get_state_vector(&kf);
x->data[0][0] = 0; // velocity
x->data[1][0] = 0; // acceleration
x->data[2][0] = 0; // jerk
/************************************************************************/
/* set covariance */
/************************************************************************/
mf16 *P = kalman_get_system_covariance(&kf);
matrix_set_symmetric(P, 0, 0, F16(0.015 * 0.015)); // var(v)
matrix_set_symmetric(P, 0, 1, 0); // cov(v,a)
matrix_set_symmetric(P, 0, 2, 0); // cov(v,j)
matrix_set_symmetric(P, 1, 1, F16(0.02484 * 0.02484)); // var(a)
matrix_set_symmetric(P, 1, 2, 0); // cov(a,j)
matrix_set_symmetric(P, 2, 2, F16(1.7901 * 1.7901)); // var(j)
/************************************************************************/
/* set input transformation */
/************************************************************************/
mf16 *B = kalman_get_input_transition(&kf);
matrix_set(B, 0, 0, fix16_one);
matrix_set(B, 1, 0, 0);
matrix_set(B, 2, 0, 0);
/************************************************************************/
/* set system process noise */
/************************************************************************/
mf16 *Q = kalman_get_input_covariance(&kf);
mf16_fill(Q, F16(0.004*0.004));
/************************************************************************/
/* set measurement transformation */
/************************************************************************/
mf16 *H = kalman_get_observation_transformation(&kfm);
matrix_set(H, 0, 0, fix16_one); // z = 1*v
matrix_set(H, 0, 1, 0); // + 0*a
matrix_set(H, 0, 2, 0); // + 0*j
matrix_set(H, 1, 0, 0); // z = 0*v
matrix_set(H, 1, 1, fix16_one); // + 1*a
matrix_set(H, 1, 2, 0); // + 0*j
matrix_set(H, 2, 0, 0); // z = 0*v
matrix_set(H, 2, 1, 0); // + 0*a
matrix_set(H, 2, 2, fix16_one); // + 1*j
/************************************************************************/
/* set process noise */
/************************************************************************/
mf16 *R = kalman_get_observation_process_noise(&kfm);
matrix_set(R, 0, 0, F16(0.0038384*0.0038384)); // var(v)
matrix_set(R, 1, 1, F16(0.024751*0.024751)); // var(v)
matrix_set(R, 2, 2, F16(0.057375*0.057375)); // var(j)
}
void VelocityFusion::SetStateTransition(milliseconds_t time)
{
mf16 *A = kalman_get_state_transition(&kf);
// set time constant
const fix16_t T = fix16_div(fix16_from_int(time.value), fix16_from_int(1000));
const fix16_t Tsquare = fix16_sq(T);
// helper
const fix16_t fix16_half = fix16_from_float(0.5);
// transition of x to v
matrix_set(A, 0, 0, 0); // zero, because we add it in with the control vector
matrix_set(A, 0, 1, T); // T
matrix_set(A, 0, 2, fix16_mul(fix16_half, Tsquare)); // 0.5 * T^2
// transition of x to a
matrix_set(A, 1, 0, 0); // 0
matrix_set(A, 1, 1, fix16_one); // 1
matrix_set(A, 1, 2, T); // T
// transition of x to j
matrix_set(A, 2, 0, 0); // 0
matrix_set(A, 2, 1, 0); // 0
matrix_set(A, 2, 2, fix16_one); // 1
}
milliseconds_t VelocityFusion::Predict(Fix16 u_velocity)
{
mf16 *u = kalman_get_input_vector(&kf);
matrix_set(u, 0, 0, u_velocity.value);
const auto now = SystemTime::get()->NowMs();
const auto difference = (now - _previousRun);
_previousRun = now;
SetStateTransition(difference);
kalman_predict(&kf);
mf16 *P = kalman_get_system_covariance(&kf);
/* fix for missing control values */
P->data[1][1] = fix16_add(P->data[1][1], F16(0.024751*0.024751));
P->data[2][2] = fix16_add(P->data[2][2], F16(0.062435*0.062435));
#if 0
/* fetch state */
mf16 *x = kalman_get_state_vector(&kf);
*velocity = matrix_get(x, 0, 0);
*acceleration = matrix_get(x, 1, 0);
*jerk = matrix_get(x, 2, 0);
#endif
return difference;
}
void VelocityFusion::Update(milliseconds_t dt, Fix16 z_velocity, Fix16 z_acceleration, Fix16 z_jerk)
{
/* update */
mf16 *z = kalman_get_observation_vector(&kfm);
matrix_set(z, 0, 0, z_velocity);
matrix_set(z, 1, 0, z_acceleration);
matrix_set(z, 2, 0, z_jerk * Fix16::OneThousand / Fix16(static_cast<int16_t>(dt.value)));
/* correct */
kalman_correct(&kf, &kfm);
#if 0
/* fetch state */
mf16 *x = kalman_get_state_vector(&kf);
*velocity = matrix_get(x, 0, 0);
*acceleration = matrix_get(x, 1, 0);
*jerk = matrix_get(x, 2, 0);
#endif
mf16 *P = kalman_get_system_covariance(&kf);
matrix_set_symmetric(P, 0, 1, 0);
matrix_set_symmetric(P, 0, 2, 0);
matrix_set_symmetric(P, 1, 2, 0);
if (kf.x.data[1][0] == 0xFFFFFFFF80000000 || kf.x.data[1][0] == 0x80000000)
{
kf.x.data[0][0] = 0;
kf.P.data[0][0] = Fix16::OneThousand;
kf.x.data[1][0] = 0;
kf.P.data[1][1] = Fix16::OneThousand;
kf.x.data[2][0] = 0;
kf.P.data[2][2] = Fix16::OneThousand;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment