Implementing Kalman filter in C++

Question

I'd like to implement an extended Kalman filter in C++ using the eigen library because I'm interested in robotics, this seems like a good exercise to get better at C++ and it seems like a fun project. I was hoping I can post my code to get some feedback on writing classes and what my next steps should be from here. So I got the equations from a class online

what I have so far is below, I've hardcoded a state vector of size 2x1 and an array of measurements as a test but would like to change it so I can declare a state vector of any size, and I'll move the array of measurements to a main.cpp file. I just did this in the beginning so I can simply declare and object of this class and quickly test out the functions, and everything seems to be working so far. What I was thinking of doing next is to make another class that takes measurements from some source and converts it into eigen matrices to pass onto this kalman filter class. The main questions I have are:

1. Should I have the measurement update and state prediction as two different functions? Does it really matter? I did that in the first place because I thought it was easier to read.

2. Should I set the size of things like the state vector in the class constructor or is it better to have something like an initializer function for that?

3. I read that it's better practice to have class members that are matrices actually be pointers to the matrix, because it makes the class lighter. What does this mean? Is that important if I want to run this on a PC vs something like a raspberry pi?

4. In the measurementUpdate function, should y, S, K be class members? It'll make the class larger, but then I wouldn't be constructing and destroying the Eigen objects when the program is looping? Is that good practice?

5. Should there be a class member that takes the measurement inputs or is it better to just pass a value to the measurement update function? Does it matter?

6. Is it even worth it to try and implement a class for this or is it better to just have a single function that implements the filter?

7. removed this one because it wasn't a question.

8. I was thinking of implementing some getter functions so I can check the state variable and covariance matrix, is it better to just make those members public and not have the getter functions?

Apologies if this is posted in the wrong place and if these are super basic questions I'm pretty new to this stuff. Thanks for all the help, all advice is appreciated.

header:

#include "eigen3/Eigen/Dense"
#include <iostream>
#include <vector>

class EKF {
public:

  EKF();
  void filter(Eigen::MatrixXd Z);

private:
  void measurementUpdate(Eigen::MatrixXd Z);
  void statePrediction();

  Eigen::MatrixXd P_; //Initial uncertainty
  Eigen::MatrixXd F_; //Linearized state approximation function
  Eigen::MatrixXd H_; //Jacobian of linearrized measurement function
  Eigen::MatrixXd R_; //Measurement uncertainty
  Eigen::MatrixXd I_; //Identity matrix
  Eigen::MatrixXd u_; //Mean of state function
  Eigen::MatrixXd x_; //Matrix of initial state variables

};  

source:

EKF::EKF() {
  double meas[5] = {1.0, 2.1, 1.6, 3.1, 2.4};
  x_.resize(2, 1);
  P_.resize(2, 2);
  u_.resize(2, 1);
  F_.resize(2, 2);
  H_.resize(1, 2);
  R_.resize(1, 1);
  I_.resize(2, 2);
  Eigen::MatrixXd Z(1, 1);
  for(int i = 0; i < 5; i++){
    Z << meas[i];
    measurementUpdate(Z);
    //statePrediction();
  }
}

void EKF::measurementUpdate(Eigen::MatrixXd Z){
  //Calculate measurement residual
  Eigen::MatrixXd y = Z - (H_ * x_);
  Eigen::MatrixXd S = H_ * P_ * H_.transpose() + R_;
  Eigen::MatrixXd K = P_ * H_.transpose() * S.inverse();

  //Calculate posterior state vector and covariance matrix
  x_ = x_ + (K * y);
  P_ = (I_ - (K * H_)) * P_;
}

void EKF::statePrediction(){
  //Predict next state vector
  x_ = (F_ * x_) + u_;
  P_ = F_ * P_ * F_.transpose();
}

void EKF::filter(Eigen::MatrixXd Z){
  measurementUpdate(Z);
  statePrediction();
} 

Answer 1

One thing to consider, which will affect the answers to ypur questions, is how 'generic' a filter you want to make.

There is no restriction in a Kalman filter that the sampling rate of the measurements be constant, nor that you get all the measurements every time. The only restriction is that the measurements appear in increasing time order. If you want to support this, then your measurement function will be passed arrays of variable sizes, and the H and R matrices will also be of variable size, and moreover the F and Q matrices (though of constant shape) will need to know the time update -- in particular you will need a function to compute Q.

As an example of what I mean, consider the example of some sort of survey boat that has a dgps sensor that gives a position every second, a gyro compass that gives the ship's heading twice a second, and a rgps system that gives the range and bearing to a towed buoy every two seconds. In this case we could get measurements like this:

at 0.0 gyro and dgps
at 0.0 gyro
at 1.0 gyro and dgps
at 1.5 gyro
at 2.0 gyro, dgps and rgps

and so on. So we get different numbers of observations at different times.

On a different topic I've always found it useful to have a way of seeing how well the filter is doing. Somewhat surprisingly the state covariance matrix is not a way of seeing this. In the linear (as opposed to extended) filter the state covariance could be computed for all times before you see any data! This is not true for the extended case as the state covariance depends on the states through the measurement Jacobian, but that is a very weak dependence on the observations. I think the most useful quality measures are those based on the measurements. Easy ones to compute are the 'innovations' -- the difference between the measured values and the values calculated using the predicted state -- and the residuals -- the difference between the measured values and the values calculated using the updated state. Each of these, over time, should have mean 0. If you want to get fancier there are the normalised residuals. If ita is the innovations vector, the normalised residuals are

T = inv(S)
u = T*ita
nr[i] = u[i]/sqrt( T[i][i])

The nice thing about the normalised residuals is that each (over time) should have mean 0 but also sd 1 -- if the filter is correctly tuned.