21 #ifndef LIBMV_TRACKING_KALMAN_FILTER_H_
22 #define LIBMV_TRACKING_KALMAN_FILTER_H_
29 template <
typename T,
int N,
int K>
33 Eigen::Matrix<T, N, 1>
mean;
39 const T* observation_data,
40 const T* process_covariance_data,
41 const T* default_measurement_covariance_data)
42 : state_transition_matrix_(
43 Eigen::Matrix<
T,
N,
N,
Eigen::RowMajor>(state_transition_data)),
47 Eigen::Matrix<
T,
N,
N,
Eigen::RowMajor>(process_covariance_data)),
48 default_measurement_covariance_(
Eigen::Matrix<
T,
K,
K,
Eigen::RowMajor>(
49 default_measurement_covariance_data)) {}
51 KalmanFilter(
const Eigen::Matrix<T, N, N>& state_transition_matrix,
52 const Eigen::Matrix<T, K, N>& observation_matrix,
53 const Eigen::Matrix<T, N, N>& process_covariance,
54 const Eigen::Matrix<T, K, K>& default_measurement_covariance)
55 : state_transition_matrix_(state_transition_matrix),
56 observation_matrix_(observation_matrix),
57 process_covariance_(process_covariance),
58 default_measurement_covariance_(default_measurement_covariance) {}
62 state->mean = state_transition_matrix_ *
state->mean;
63 state->covariance = state_transition_matrix_ *
state->covariance *
64 state_transition_matrix_.transpose() +
69 void Update(
const Eigen::Matrix<T, K, 1>& measurement_mean,
70 const Eigen::Matrix<T, K, K>& measurement_covariance,
73 Eigen::Matrix<T, K, 1> innovation_mean =
74 measurement_mean - observation_matrix_ *
state->mean;
75 Eigen::Matrix<T, K, K> innovation_covariance =
76 observation_matrix_ *
state->covariance *
77 observation_matrix_.transpose() +
78 measurement_covariance;
81 Eigen::Matrix<T, 6, 2> kalman_gain =
state->covariance *
82 observation_matrix_.transpose() *
83 innovation_covariance.inverse();
86 state->mean += kalman_gain * innovation_mean;
87 state->covariance = (Eigen::Matrix<T, N, N>::Identity() -
88 kalman_gain * observation_matrix_) *
93 const Eigen::Matrix<T, K, 1>& measurement_mean)
const {
94 Update(
state, measurement_mean, default_measurement_covariance_);
98 const Eigen::Matrix<T, N, N> state_transition_matrix_;
99 const Eigen::Matrix<T, K, N> observation_matrix_;
100 const Eigen::Matrix<T, N, N> process_covariance_;
101 const Eigen::Matrix<T, K, K> default_measurement_covariance_;
void Update(State *state, const Eigen::Matrix< T, K, 1 > &measurement_mean) const
void Step(State *state) const
KalmanFilter(const T *state_transition_data, const T *observation_data, const T *process_covariance_data, const T *default_measurement_covariance_data)
void Update(const Eigen::Matrix< T, K, 1 > &measurement_mean, const Eigen::Matrix< T, K, K > &measurement_covariance, State *state) const
KalmanFilter(const Eigen::Matrix< T, N, N > &state_transition_matrix, const Eigen::Matrix< T, K, N > &observation_matrix, const Eigen::Matrix< T, N, N > &process_covariance, const Eigen::Matrix< T, K, K > &default_measurement_covariance)
Eigen::Matrix< T, N, N > covariance
Eigen::Matrix< T, N, 1 > mean