-
Notifications
You must be signed in to change notification settings - Fork 2
/
kalman_filter.h
74 lines (57 loc) · 1.83 KB
/
kalman_filter.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
#pragma once
#include <iostream>
#include <eigen3/Eigen/Dense>
// abstract class for Kalman filter
// implementation could be KF/EKF/UKF...
class KalmanFilter {
public:
/**
* user need to define H matrix & R matrix
* @param num_states
* @param num_obs
*/
// constructor
explicit KalmanFilter(unsigned int num_states, unsigned int num_obs);
// destructor
virtual ~KalmanFilter() = default;
/**
* Coast state and state covariance using the process model
* User can use this function without change the internal
* tracking state x_
*/
virtual void Coast();
/**
* Predict without measurement update
*/
void Predict();
/**
* This function maps the true state space into the observed space
* using the observation model
* User can implement their own method for more complicated models
*/
virtual Eigen::VectorXd PredictionToObservation(const Eigen::VectorXd &state);
/**
* Updates the state by using Extended Kalman Filter equations
* @param z The measurement at k+1
*/
virtual void Update(const Eigen::VectorXd &z);
/**
* Calculate marginal log-likelihood to evaluate different parameter choices
*/
float CalculateLogLikelihood(const Eigen::VectorXd& y, const Eigen::MatrixXd& S);
// State vector
Eigen::VectorXd x_, x_predict_;
// Error covariance matrix
Eigen::MatrixXd P_, P_predict_;
// State transition matrix
Eigen::MatrixXd F_;
// Covariance matrix of process noise
Eigen::MatrixXd Q_;
// measurement matrix
Eigen::MatrixXd H_;
// covariance matrix of observation noise
Eigen::MatrixXd R_;
unsigned int num_states_, num_obs_;
float log_likelihood_delta_;
float NIS_;
};