tiny_ekf.cpp File Reference

#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include "tiny_ekf.h"

Classes
struct	ekf_t

Functions
void	ekf_init (void *v, int n, int m)
int	ekf_step (void *v, double *z)

Function Documentation

ekf_init()

void ekf_init	(	void *	ekf,
		int	n,
		int	m
	)

Initializes an EKF structure.

Parameters

ekf	pointer to EKF structure to initialize
n	number of state variables
m	number of observables

ekf should be a pointer to a structure defined as follows, where N and M are constants:

    int n;           // number of state values
    int m;           // number of observables

    double x[N];     // state vector

    double P[N][N];  // prediction error covariance
    double Q[N][N];  // process noise covariance
    double R[M][M];  // measurement error covariance

    double G[N][M];  // Kalman gain; a.k.a. K

    double F[N][N];  // Jacobian of process model
    double H[M][N];  // Jacobian of measurement model

    double Ht[N][M]; // transpose of measurement Jacobian
    double Ft[N][N]; // transpose of process Jacobian
    double Pp[N][N]; // P, post-prediction, pre-update

    double fx[N];   // output of user defined f() state-transition function
    double hx[M];   // output of user defined h() measurement function

    // temporary storage
    double tmp0[N][N];
    double tmp1[N][Msta];
    double tmp2[M][N];
    double tmp3[M][M];
    double tmp4[M][M];
    double tmp5[M];
  

ekf_step()

int ekf_step	(	void *	ekf,
		double *	z
	)

Runs one step of EKF prediction and update. Your code should first build a model, setting the contents of ekf.fx, ekf.F, ekf.hx, and ekf.H to appropriate values.

Parameters

ekf	pointer to structure EKF
z	array of measurement (observation) values

Returns

0 on success, 1 on failure caused by non-positive-definite matrix.