-
Notifications
You must be signed in to change notification settings - Fork 0
/
kf.h
76 lines (60 loc) · 1.53 KB
/
kf.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
#pragma once
#ifndef kf
#define kf
#include "Eigen/Dense"
class kalman {
public:
// state vector
Eigen::VectorXd state_vec;
// state covariance matrix
Eigen::MatrixXd state_covar_mat;
// state transistion matrix
Eigen::MatrixXd state_trans_mat;
// process covariance matrix
Eigen::MatrixXd process_covar_mat;
// measurement matrix
Eigen::MatrixXd measurement_mat;
// measurement covariance matrix
Eigen::MatrixXd measurement_covar_mat;
/**
* Constructor
*/
kalman();
/**
* Destructor
*/
virtual ~kalman();
/**
* Init Initializes Kalman filter
* @param x_in Initial state
* @param P_in Initial state covariance
* @param F_in Transition matrix
* @param H_in Measurement matrix
* @param R_in Measurement covariance matrix
* @param Q_in Process covariance matrix
*/
void init(Eigen::VectorXd &x_in, Eigen::MatrixXd &P_in, Eigen::MatrixXd &F_in,
Eigen::MatrixXd &H_in, Eigen::MatrixXd &R_in, Eigen::MatrixXd &Q_in);
/**
* Prediction Predicts the state and the state covariance
* using the process model
* @param delta_T Time between k and k+1 in s
*/
void predict();
/**
* Updates the state by using standard Kalman Filter equations
* @param z The measurement at k+1
*/
void update(const Eigen::VectorXd &z);
/**
* Updates the state by using Extended Kalman Filter equations
* @param z The measurement at k+1
*/
void update_ekf(const Eigen::VectorXd &z);
/**
* Universal update Kalman Filter step. Equations from the lectures
* @param y The error
*/
void univ_update_kf(const Eigen::VectorXd &y);
};
#endif