kalman.c

#include "kalman.h"
#include <assert.h>
#include "fixkalman.h"

// create the filter structure
#define KALMAN_NAME sonar
#define KALMAN_NUM_STATES 5
#define KALMAN_NUM_INPUTS 0

kalman16_uc_t kf;

#define KALMAN_MEASUREMENT_NAME position
#define KALMAN_NUM_MEASUREMENTS 4
kalman16_observation_t kfm;

#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

#ifndef FIXMATRIX_MAX_SIZE
#error FIXMATRIX_MAX_SIZE must be defined and greater or equal to the number of states, inputs and measurements.
#endif

#if (FIXMATRIX_MAX_SIZE < KALMAN_NUM_STATES) || (FIXMATRIX_MAX_SIZE < KALMAN_NUM_INPUTS) || (FIXMATRIX_MAX_SIZE < KALMAN_NUM_MEASUREMENTS)
#error FIXMATRIX_MAX_SIZE must be greater or equal to the number of states, inputs and measurements.
#endif

static int16_t old_ground_distance = 0;

/*!
* \brief Initializes the gravity Kalman filter
*/
void kalman_sonar_init()
{
    /************************************************************************/
    /* initialize the filter structures                                     */
    /************************************************************************/

    kalman_filter_initialize_uc(&kf, KALMAN_NUM_STATES);

    kalman_observation_initialize(&kfm, KALMAN_NUM_STATES, KALMAN_NUM_MEASUREMENTS);

    /************************************************************************/
    /* set initial state                                                    */
    /************************************************************************/

    mf16 *x = kalman_get_state_vector_uc(&kf);

    x->data[0][0] = 0; // s_i
    x->data[1][0] = 0; // v_i
    x->data[2][0] = 0; // g_i
    x->data[3][0] = 0; // vx_i
    x->data[4][0] = 0; // vy_i

    /************************************************************************/
    /* set state transition                                                 */
    /************************************************************************/

    mf16 *A = kalman_get_state_transition_uc(&kf);

    // set time constant
    const fix16_t T = fix16_div(fix16_one,fix16_from_float(200.0));
    const fix16_t Tsquare = fix16_sq(T);

    // helper
    const fix16_t fix16_half = fix16_from_float(0.5);

    // transition of x to s
    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
    matrix_set(A, 0, 3, 0);   // 0
    matrix_set(A, 0, 4, 0);   // 0

    // transition of x to v
    matrix_set(A, 1, 0, 0);   // 0
    matrix_set(A, 1, 1, fix16_one);   // 1
    matrix_set(A, 1, 2, T);   // T
    matrix_set(A, 1, 3, 0);   // 0
    matrix_set(A, 1, 4, 0);   // 0

    // transition of x to g
    matrix_set(A, 2, 0, 0);   // 0
    matrix_set(A, 2, 1, 0);   // 0
    matrix_set(A, 2, 2, fix16_one);   // 1
    matrix_set(A, 2, 3, 0);   // 0
    matrix_set(A, 2, 4, 0);   // 0

    // transition of x to xv
    matrix_set(A, 3, 0, 0);   // 0
    matrix_set(A, 3, 1, 0);   // 0
    matrix_set(A, 3, 2, 0);   // 0
    matrix_set(A, 3, 3, fix16_one);   // 1
    matrix_set(A, 3, 4, 0);   // 0

    // transition of x to yv
    matrix_set(A, 4, 0, 0);   // 0
    matrix_set(A, 4, 1, 0);   // 0
    matrix_set(A, 4, 2, 0);   // 0
    matrix_set(A, 4, 3, 0);   // 0
    matrix_set(A, 4, 4, fix16_one);   // 1


    /************************************************************************/
    /* set covariance                                                       */
    /************************************************************************/
    mf16 *P = kalman_get_system_covariance_uc(&kf);

    matrix_set_symmetric(P, 0, 0, fix16_half);   // var(s)
    matrix_set_symmetric(P, 0, 1, 0);   // cov(s,v)
    matrix_set_symmetric(P, 0, 2, 0);   // cov(s,g)
    matrix_set_symmetric(P, 0, 3, 0);   // cov(s,xv)
    matrix_set_symmetric(P, 0, 4, 0);   // cov(s,yv)

    matrix_set_symmetric(P, 1, 1, fix16_half);   // var(v)
    matrix_set_symmetric(P, 1, 2, 0);   // cov(v,g)
    matrix_set_symmetric(P, 1, 3, 0);   // cov(v,xv)
    matrix_set_symmetric(P, 1, 4, 0);   // cov(v,yv)

    matrix_set_symmetric(P, 2, 2, fix16_one);   // var(g)
    matrix_set_symmetric(P, 2, 3, 0);   // cov(g,xv)
    matrix_set_symmetric(P, 2, 4, 0);   // cov(g,yv)

    matrix_set_symmetric(P, 3, 3, fix16_half);   // var(xv)
    matrix_set_symmetric(P, 3, 4, 0);   // cov(xv,yv)

    matrix_set_symmetric(P, 4, 4, fix16_half);   // var(yv)


    /************************************************************************/
    /* set system process noise                                             */
    /************************************************************************/
    mf16 *Q = kalman_get_system_process_noise_uc(&kf);
    //mf16_fill(Q, F16(0.001));
    matrix_set_symmetric(Q, 0, 0, F16(0.001));
    matrix_set_symmetric(Q, 0, 1, 0);
    matrix_set_symmetric(Q, 0, 2, 0);
    matrix_set_symmetric(Q, 0, 3, 0);
    matrix_set_symmetric(Q, 0, 4, 0);

    matrix_set_symmetric(Q, 1, 1, F16(0.001));
    matrix_set_symmetric(Q, 1, 2, 0);
    matrix_set_symmetric(Q, 1, 3, 0);
    matrix_set_symmetric(Q, 1, 4, 0);

    matrix_set_symmetric(Q, 2, 2, F16(0.001));
    matrix_set_symmetric(Q, 2, 3, 0);
    matrix_set_symmetric(Q, 2, 4, 0);

    matrix_set_symmetric(Q, 3, 3, F16(0.001));
    matrix_set_symmetric(Q, 3, 4, 0);

    matrix_set_symmetric(Q, 4, 4, F16(0.001));

    /************************************************************************/
    /* set measurement transformation                                       */
    /************************************************************************/
    mf16 *H = kalman_get_observation_transformation(&kfm);

    matrix_set(H, 0, 0, fix16_one);     // z = 1*s
    matrix_set(H, 0, 1, 0);     //   + 0*v
    matrix_set(H, 0, 2, 0);     //   + 0*g
    matrix_set(H, 0, 3, 0);     //   + 0*xv
    matrix_set(H, 0, 4, 0);     //   + 0*yv

    matrix_set(H, 1, 0, 0);     //     0*s
    matrix_set(H, 1, 1, 0);     //   + 0*v
    matrix_set(H, 1, 2, fix16_one);     //   + 1*g
    matrix_set(H, 1, 3, 0);     //   + 0*xv
    matrix_set(H, 1, 4, 0);     //   + 0*yv

    matrix_set(H, 2, 0, 0);     //     0*s
    matrix_set(H, 2, 1, 0);     //   + 0*v
    matrix_set(H, 2, 2, 0);     //   + 0*g
    matrix_set(H, 2, 3, fix16_one);     //   + 1*xv
    matrix_set(H, 2, 4, 0);     //   + 0*yv

    matrix_set(H, 3, 0, 0);     //     0*s
    matrix_set(H, 3, 1, 0);     //   + 0*v
    matrix_set(H, 3, 2, 0);     //   + 0*g
    matrix_set(H, 3, 3, 0);     //   + 0*xv
    matrix_set(H, 3, 4, fix16_one);     //   + 1*yv


    /************************************************************************/
    /* set process noise                                                    */
    /************************************************************************/
    mf16 *R = kalman_get_observation_process_noise(&kfm);

    matrix_set(R, 0, 0, fix16_from_float(0.04));     // var(s)
    matrix_set(R, 1, 1, fix16_from_float(2.0));     // var(g)
    matrix_set(R, 2, 2, fix16_from_float(0.2));     // var(xv)
    matrix_set(R, 3, 3, fix16_from_float(0.2));     // var(yv)
}

void kalman_sonar_update( int16_t ground_distance,
                          int16_t z_acceleration,
                          int16_t x_velocity,
                          int16_t x_acceleration,
                          int16_t y_velocity,
                          int16_t y_acceleration)
{
  mf16 *z = kalman_get_observation_vector(&kfm);

  if (ground_distance <= 0) {
    ground_distance = old_ground_distance;
  } else {
    old_ground_distance = ground_distance;
  }
  fix16_t measurement_height = fix16_from_int(ground_distance);
  matrix_set(z, 0, 0, measurement_height);

  fix16_t measurement_z_acceleration = fix16_from_int(z_acceleration);
  matrix_set(z, 1, 0, measurement_z_acceleration);

  fix16_t measurement_x_velocity = fix16_from_int(x_velocity);
  matrix_set(z, 2, 0, measurement_x_velocity);

  fix16_t measurement_y_velocity = fix16_from_int(y_velocity);
  matrix_set(z, 3, 0, measurement_y_velocity);

  kalman_correct_uc(&kf, &kfm);
}

void kalman_sonar_predict()
{
  kalman_predict_uc(&kf);
}

int16_t kalman_sonar_get_height(void)
{
  mf16 *x = kalman_get_state_vector_uc(&kf);
  const fix16_t estimate = x->data[0][0];
  return fix16_to_int(estimate);
}

int16_t kalman_sonar_get_z_velocity(void)
{
  mf16 *x = kalman_get_state_vector_uc(&kf);
  const fix16_t estimate = x->data[1][0];
  return fix16_to_int(estimate);
}

int16_t kalman_sonar_get_z_acceleration(void)
{
  mf16 *x = kalman_get_state_vector_uc(&kf);
  const fix16_t estimate = x->data[2][0];
  return fix16_to_int(estimate);
}

int16_t kalman_sonar_get_x_velocity(void)
{
  mf16 *x = kalman_get_state_vector_uc(&kf);
  const fix16_t estimate = x->data[3][0];
  return fix16_to_int(estimate);
}

int16_t kalman_sonar_get_y_velocity(void)
{
  mf16 *x = kalman_get_state_vector_uc(&kf);
  const fix16_t estimate = x->data[4][0];
  return fix16_to_int(estimate);
}