-
Notifications
You must be signed in to change notification settings - Fork 0
/
kalman_filter.cpp
118 lines (90 loc) · 2.48 KB
/
kalman_filter.cpp
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
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
#include "measurement_package.h"
#include "kalman_filter.h"
#include <iostream>
// #include "Eigen"
#include "Eigen/Dense"
using Eigen::MatrixXd;
using Eigen::VectorXd;
// Please note that the Eigen library does not initialize
// VectorXd or MatrixXd objects with zeros upon creation.
KalmanFilter::KalmanFilter()
{
dt = 0.1;
is_initialized_ = false;
previous_timestamp_ = 0;
F_ = MatrixXd(2, 2);
F_ << 1, dt,
0, 1;
x_ = VectorXd(2);
x_.fill(0);
Q_ = MatrixXd(2, 2);
Q_ << 0.01, 0,
0, 0.01;
// Q_ << noise_ax*noise_ax, 0,
// 0, noise_ay*noise_ay;
H_ = MatrixXd(2, 2);
H_ << 1, 0,
0, 1;
R_ = MatrixXd(2, 2);
R_ << 0.01, 0,
0, 0.01;
// R_ = MatrixXd(1, 1);
// R_ << 1;
P_ = MatrixXd(2, 2);
P_ << 100, 0,
0, 100;
}
KalmanFilter::~KalmanFilter() {}
void KalmanFilter::perform_kf(const MeasurementPackage &measurement_pack)
{
if (!is_initialized_)
{
// first measurement
x_ = VectorXd(2);
x_ << 1, 1;
previous_timestamp_ = measurement_pack.timestamp_;
if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR)
{
x_ << measurement_pack.raw_measurements_[0], measurement_pack.raw_measurements_[1];
}
else if (measurement_pack.sensor_type_ == MeasurementPackage::LASER)
{
x_ << measurement_pack.raw_measurements_[0], measurement_pack.raw_measurements_[1];
}
// done initializing, no need to predict or update
is_initialized_ = true;
return;
}
// dt = (measurement_pack.timestamp_ - previous_timestamp_) / 1000000.0; //dt - expressed in seconds
previous_timestamp_ = measurement_pack.timestamp_;
this->Predict();
this->Update(measurement_pack.raw_measurements_);
}
void KalmanFilter::Predict()
{
// =======================
// predicting the state
// =======================
x_ = F_ * x_;
MatrixXd Ft = F_.transpose();
P_ = F_ * P_ * Ft ;
}
void KalmanFilter::Update(const VectorXd &z) {
// ===================================================
// update the state by using Kalman Filter equations
// ===================================================
VectorXd z_predicted = H_ * x_;
VectorXd y = z - z_predicted;
MatrixXd Ht = H_.transpose();
MatrixXd PHt = P_ * Ht;
MatrixXd S = H_ * PHt + R_;
MatrixXd Si = S.inverse();
MatrixXd K = PHt * Si;
// ==============
// new estimate
// ==============
x_ = x_ + (K * y);
long x_size = x_.size();
MatrixXd I = MatrixXd::Identity(x_size, x_size);
P_ = (I - K * H_) * P_;
}