Blender  V3.3
kalman_filter.h
Go to the documentation of this file.
1 // Copyright (c) 2014 libmv authors.
2 //
3 // Permission is hereby granted, free of charge, to any person obtaining a copy
4 // of this software and associated documentation files (the "Software"), to
5 // deal in the Software without restriction, including without limitation the
6 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
7 // sell copies of the Software, and to permit persons to whom the Software is
8 // furnished to do so, subject to the following conditions:
9 //
10 // The above copyright notice and this permission notice shall be included in
11 // all copies or substantial portions of the Software.
12 //
13 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
18 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
19 // IN THE SOFTWARE.
20 
21 #ifndef LIBMV_TRACKING_KALMAN_FILTER_H_
22 #define LIBMV_TRACKING_KALMAN_FILTER_H_
23 
24 #include "libmv/numeric/numeric.h"
25 
26 namespace mv {
27 
28 // A Kalman filter with order N and observation size K.
29 template <typename T, int N, int K>
30 class KalmanFilter {
31  public:
32  struct State {
33  Eigen::Matrix<T, N, 1> mean;
34  Eigen::Matrix<T, N, N> covariance;
35  };
36 
37  // Initialize from row-major data; convenient for constant initializers.
38  KalmanFilter(const T* state_transition_data,
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)),
44  observation_matrix_(
45  Eigen::Matrix<T, K, N, Eigen::RowMajor>(observation_data)),
46  process_covariance_(
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)) {}
50 
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) {}
59 
60  // Advances the system according to the current state estimate.
61  void Step(State* state) const {
62  state->mean = state_transition_matrix_ * state->mean;
63  state->covariance = state_transition_matrix_ * state->covariance *
64  state_transition_matrix_.transpose() +
65  process_covariance_;
66  }
67 
68  // Updates a state with a new measurement.
69  void Update(const Eigen::Matrix<T, K, 1>& measurement_mean,
70  const Eigen::Matrix<T, K, K>& measurement_covariance,
71  State* state) const {
72  // Calculate the innovation, which is a distribution over prediction error.
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;
79 
80  // Calculate the Kalman gain.
81  Eigen::Matrix<T, 6, 2> kalman_gain = state->covariance *
82  observation_matrix_.transpose() *
83  innovation_covariance.inverse();
84 
85  // Update the state mean and covariance.
86  state->mean += kalman_gain * innovation_mean;
87  state->covariance = (Eigen::Matrix<T, N, N>::Identity() -
88  kalman_gain * observation_matrix_) *
89  state->covariance;
90  }
91 
93  const Eigen::Matrix<T, K, 1>& measurement_mean) const {
94  Update(state, measurement_mean, default_measurement_covariance_);
95  }
96 
97  private:
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_;
102 };
103 
104 } // namespace mv
105 
106 #endif // LIBMV_TRACKING_KALMAN_FILTER_H_
#define K(key)
void Update(State *state, const Eigen::Matrix< T, K, 1 > &measurement_mean) const
Definition: kalman_filter.h:92
void Step(State *state) const
Definition: kalman_filter.h:61
KalmanFilter(const T *state_transition_data, const T *observation_data, const T *process_covariance_data, const T *default_measurement_covariance_data)
Definition: kalman_filter.h:38
void Update(const Eigen::Matrix< T, K, 1 > &measurement_mean, const Eigen::Matrix< T, K, K > &measurement_covariance, State *state) const
Definition: kalman_filter.h:69
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)
Definition: kalman_filter.h:51
const int state
#define N
#define T
Eigen::Matrix< T, N, N > covariance
Definition: kalman_filter.h:34
Eigen::Matrix< T, N, 1 > mean
Definition: kalman_filter.h:33