FilesExpand file tree

 main

/

linear_kf.cpp

Copy path

Blame

More file actions

Blame

More file actions

Latest commit

 

History

History

History

82 lines (60 loc) · 2.02 KB

 main

/

linear_kf.cpp

Copy path

Top

File metadata and controls

• Code
• Blame

82 lines (60 loc) · 2.02 KB

Raw

Copy raw file

Download raw file

Open symbols panel

Edit and raw actions

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

// Implementation of LinearKF: predict() and update() using standard KF equations.

// Matrix setup (F, H, Q, R, P) happens in the constructor.

#include "linear_kf.hpp"

LinearKF::LinearKF(Eigen::Vector3d position)

{

// Tunable Params

Eigen::VectorXd sigma_squared_state(6);

sigma_squared_state<< 100, 100, 100, 20, 20, 5; // sigma squared for x, y, z, vx, vy, vz

Eigen::VectorXd sigma_squared_measurement(3);

sigma_squared_measurement<< 0.1, 0.1, 0.1; // sigma squared for x, y, z

Eigen::VectorXd sigma_squared_process(6);

sigma_squared_process << 1, 1, 1, 1, 1, 1;

// Kalman Filter Matrices Initialization

x_ = Eigen::VectorXd::Zero(6);

x_.head(3) = position;

P_ = sigma_squared_state.asDiagonal();

// Partially filling F matrix.

// Only filling in the constant terms, the time dependent terms will be filled in later

F_ = Eigen::MatrixXd::Identity(6, 6);

Q_ = sigma_squared_process.asDiagonal();

H_ = Eigen::MatrixXd::Zero(3, 6);

H_.topLeftCorner(3, 3) = Eigen::Matrix3d::Identity();

R_ = sigma_squared_measurement.asDiagonal();

}

void LinearKF::predict(double dt)

{

// Updating relevant parts of the F matrix that depend on dt

F_.topRightCorner(3,3) = dt*Eigen::Matrix3d::Identity(3,3);

x_ = F_*x_;

P_ = F_*P_*F_.transpose() + Q_;

}

void LinearKF::update(const Eigen::VectorXd& z, const double yaw)

{

Eigen::VectorXd y = z - H_*x_;

Eigen::MatrixXd S = H_*P_*H_.transpose() + R_;

Eigen::MatrixXd K = P_*H_.transpose()*S.inverse();

Eigen::MatrixXd I = Eigen::MatrixXd::Identity(6, 6);

x_ = x_ + K*y;

P_ = (I - K*H_)*P_;

yaw_ = yaw;

}

Eigen::Vector3d LinearKF::get_position() const

{

return x_.head(3);

}

Eigen::MatrixXd LinearKF::get_covariance() const

{

return P_;

}

Innovation LinearKF::compute_innovation(const Eigen::VectorXd& z) const

{

Innovation innov;

innov.y = z - H_ * x_;

innov.S = H_ * P_ * H_.transpose() + R_;

return innov;

}

double LinearKF::get_yaw() const

{

return yaw_;

}