forked from philbort/udacity_self_driving_car
-
Notifications
You must be signed in to change notification settings - Fork 0
/
kalman_filter.h
78 lines (66 loc) · 2.55 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
75
76
77
78
#ifndef KALMAN_FILTER_H_
#define KALMAN_FILTER_H_
#include "Eigen/Dense"
class KalmanFilter
{
public:
// state vector
Eigen::VectorXd x_;
// state covariance matrix
Eigen::MatrixXd P_;
// -----------------------------------------------------------------------------
// @brief Constructor
//
// @param[in] n State vector dimension
// -----------------------------------------------------------------------------
KalmanFilter(int n);
// -----------------------------------------------------------------------------
// @brief Destructor
// -----------------------------------------------------------------------------
virtual ~KalmanFilter() {}
// -----------------------------------------------------------------------------
// @brief Init
//
// Initialize Kalman filter state and state covariance matrix
//
// @param[in] x_in Initial state vector
// @param[in] P_in Initial state vector covariance matrix
// -----------------------------------------------------------------------------
void Init(const Eigen::VectorXd &x_in,
const Eigen::MatrixXd &P_in);
// -----------------------------------------------------------------------------
// @brief Predict
//
// Kalman filter time update (prediction)
//
// @param[in] F State transition matrix
// @param[in] Q State covariance matrix
// -----------------------------------------------------------------------------
void Predict(const Eigen::MatrixXd &F,
const Eigen::MatrixXd &Q);
// -----------------------------------------------------------------------------
// @brief Update
//
// Linear Kalman filter measurement update
//
// @param[in] z Measurement vector
// @param[in] H Measurement design matrix
// @param[in] R Measurement noise covariance matrix
// -----------------------------------------------------------------------------
void Update(const Eigen::VectorXd &z,
const Eigen::MatrixXd &H,
const Eigen::MatrixXd &R);
// -----------------------------------------------------------------------------
// @brief Update
//
// Extended Kalman filter measurement update
//
// @param[in] z Measurement vector
// @param[in] H Measurement design matrix
// @param[in] R Measurement noise covariance matrix
// -----------------------------------------------------------------------------
void UpdateEKF(const Eigen::VectorXd &z,
const Eigen::MatrixXd &H,
const Eigen::MatrixXd &R);
};
#endif /* KALMAN_FILTER_H_ */