unscented_kalman_filter.h
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
9  *
10  * Unless required by applicable law or agreed to in writing, software
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16
17 #ifndef CARTOGRAPHER_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_
18 #define CARTOGRAPHER_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_
19
20 #include <algorithm>
21 #include <cmath>
22 #include <functional>
23 #include <vector>
24
25 #include "Eigen/Cholesky"
26 #include "Eigen/Core"
27 #include "Eigen/Eigenvalues"
28
29 #include "glog/logging.h"
30
31 namespace cartographer {
32 namespace kalman_filter {
33
34 template <typename FloatType>
35 constexpr FloatType sqr(FloatType a) {
36  return a * a;
37 }
38
39 template <typename FloatType, int N>
40 Eigen::Matrix<FloatType, N, N> OuterProduct(
41  const Eigen::Matrix<FloatType, N, 1>& v) {
42  return v * v.transpose();
43 }
44
45 // Checks if 'A' is a symmetric matrix.
46 template <typename FloatType, int N>
47 void CheckSymmetric(const Eigen::Matrix<FloatType, N, N>& A) {
48  // This should be pretty much Eigen::Matrix<>::Zero() if the matrix is
49  // symmetric.
50  const FloatType norm = (A - A.transpose()).norm();
51  CHECK(!std::isnan(norm) && std::abs(norm) < 1e-5)
52  << "Symmetry check failed with norm: '" << norm << "' from matrix:\n"
53  << A;
54 }
55
56 // Returns the matrix square root of a symmetric positive semidefinite matrix.
57 template <typename FloatType, int N>
58 Eigen::Matrix<FloatType, N, N> MatrixSqrt(
59  const Eigen::Matrix<FloatType, N, N>& A) {
60  CheckSymmetric(A);
61
63  adjoint_eigen_solver((A + A.transpose()) / 2.);
64  const auto& eigenvalues = adjoint_eigen_solver.eigenvalues();
65  CHECK_GT(eigenvalues.minCoeff(), -1e-5)
66  << "MatrixSqrt failed with negative eigenvalues: "
67  << eigenvalues.transpose();
68
71  .cwiseMax(Eigen::Matrix<FloatType, N, 1>::Zero())
72  .cwiseSqrt()
73  .asDiagonal() *
75 }
76
77 // Implementation of a Kalman filter. We follow the nomenclature from
78 // Thrun, S. et al., Probabilistic Robotics, 2006.
79 //
80 // Extended to handle non-additive noise/sensors inspired by Kraft, E., A
81 // Quaternion-based Unscented Kalman Filter for Orientation Tracking.
82 template <typename FloatType, int N>
84  public:
85  using StateType = Eigen::Matrix<FloatType, N, 1>;
86  using StateCovarianceType = Eigen::Matrix<FloatType, N, N>;
87
89  const GaussianDistribution<FloatType, N>& initial_belief,
90  std::function<StateType(const StateType& state, const StateType& delta)>
91  add_delta = [](const StateType& state,
92  const StateType& delta) { return state + delta; },
93  std::function<StateType(const StateType& origin, const StateType& target)>
94  compute_delta =
95  [](const StateType& origin, const StateType& target) {
96  return target - origin;
97  })
98  : belief_(initial_belief),
100  compute_delta_(compute_delta) {}
101
102  // Does the control/prediction step for the filter. The control must be
103  // implicitly added by the function g which also does the state transition.
104  // 'epsilon' is the additive combination of control and model noise.
105  void Predict(std::function<StateType(const StateType&)> g,
106  const GaussianDistribution<FloatType, N>& epsilon) {
107  CheckSymmetric(epsilon.GetCovariance());
108
109  // Get the state mean and matrix root of its covariance.
110  const StateType& mu = belief_.GetMean();
111  const StateCovarianceType sqrt_sigma = MatrixSqrt(belief_.GetCovariance());
112
113  std::vector<StateType> Y;
114  Y.reserve(2 * N + 1);
115  Y.emplace_back(g(mu));
116
117  const FloatType kSqrtNPlusLambda = std::sqrt(N + kLambda);
118  for (int i = 0; i < N; ++i) {
119  // Order does not matter here as all have the same weights in the
120  // summation later on anyways.
123  }
124  const StateType new_mu = ComputeMean(Y);
125
126  StateCovarianceType new_sigma =
127  kCovWeight0 * OuterProduct<FloatType, N>(compute_delta_(new_mu, Y[0]));
128  for (int i = 0; i < N; ++i) {
129  new_sigma += kCovWeightI * OuterProduct<FloatType, N>(
130  compute_delta_(new_mu, Y[2 * i + 1]));
131  new_sigma += kCovWeightI * OuterProduct<FloatType, N>(
132  compute_delta_(new_mu, Y[2 * i + 2]));
133  }
134  CheckSymmetric(new_sigma);
135
136  belief_ = GaussianDistribution<FloatType, N>(new_mu, new_sigma) + epsilon;
137  }
138
139  // The observation step of the Kalman filter. 'h' transfers the state
140  // into an observation that should be zero, i.e., the sensor readings should
141  // be included in this function already. 'delta' is the measurement noise and
142  // must have zero mean.
143  template <int K>
144  void Observe(
145  std::function<Eigen::Matrix<FloatType, K, 1>(const StateType&)> h,
146  const GaussianDistribution<FloatType, K>& delta) {
147  CheckSymmetric(delta.GetCovariance());
148  // We expect zero mean delta.
149  CHECK_NEAR(delta.GetMean().norm(), 0., 1e-9);
150
151  // Get the state mean and matrix root of its covariance.
152  const StateType& mu = belief_.GetMean();
153  const StateCovarianceType sqrt_sigma = MatrixSqrt(belief_.GetCovariance());
154
155  // As in Kraft's paper, we compute W containing the zero-mean sigma points,
156  // since this is all we need.
157  std::vector<StateType> W;
158  W.reserve(2 * N + 1);
159  W.emplace_back(StateType::Zero());
160
161  std::vector<Eigen::Matrix<FloatType, K, 1>> Z;
162  Z.reserve(2 * N + 1);
163  Z.emplace_back(h(mu));
164
165  Eigen::Matrix<FloatType, K, 1> z_hat = kMeanWeight0 * Z[0];
166  const FloatType kSqrtNPlusLambda = std::sqrt(N + kLambda);
167  for (int i = 0; i < N; ++i) {
168  // Order does not matter here as all have the same weights in the
169  // summation later on anyways.
170  W.emplace_back(kSqrtNPlusLambda * sqrt_sigma.col(i));
172
173  W.emplace_back(-kSqrtNPlusLambda * sqrt_sigma.col(i));
175
176  z_hat += kMeanWeightI * Z[2 * i + 1];
177  z_hat += kMeanWeightI * Z[2 * i + 2];
178  }
179
180  Eigen::Matrix<FloatType, K, K> S =
181  kCovWeight0 * OuterProduct<FloatType, K>(Z[0] - z_hat);
182  for (int i = 0; i < N; ++i) {
183  S += kCovWeightI * OuterProduct<FloatType, K>(Z[2 * i + 1] - z_hat);
184  S += kCovWeightI * OuterProduct<FloatType, K>(Z[2 * i + 2] - z_hat);
185  }
186  CheckSymmetric(S);
187  S += delta.GetCovariance();
188
189  Eigen::Matrix<FloatType, N, K> sigma_bar_xz =
190  kCovWeight0 * W[0] * (Z[0] - z_hat).transpose();
191  for (int i = 0; i < N; ++i) {
192  sigma_bar_xz +=
193  kCovWeightI * W[2 * i + 1] * (Z[2 * i + 1] - z_hat).transpose();
194  sigma_bar_xz +=
195  kCovWeightI * W[2 * i + 2] * (Z[2 * i + 2] - z_hat).transpose();
196  }
197
198  const Eigen::Matrix<FloatType, N, K> kalman_gain =
199  sigma_bar_xz * S.inverse();
200  const StateCovarianceType new_sigma =
201  belief_.GetCovariance() - kalman_gain * S * kalman_gain.transpose();
202  CheckSymmetric(new_sigma);
203
205  add_delta_(mu, kalman_gain * -z_hat), new_sigma);
206  }
207
209  return belief_;
210  }
211
212  private:
214  const std::vector<StateType>& states) {
215  StateType weighted_error =
216  kMeanWeight0 * compute_delta_(mean_estimate, states[0]);
217  for (int i = 1; i != 2 * N + 1; ++i) {
218  weighted_error += kMeanWeightI * compute_delta_(mean_estimate, states[i]);
219  }
220  return weighted_error;
221  }
222
223  // Algorithm for computing the mean of non-additive states taken from Kraft's
224  // Section 3.4, adapted to our implementation.
225  StateType ComputeMean(const std::vector<StateType>& states) {
226  CHECK_EQ(states.size(), 2 * N + 1);
227  StateType current_estimate = states[0];
228  StateType weighted_error = ComputeWeightedError(current_estimate, states);
229  int iterations = 0;
230  while (weighted_error.norm() > 1e-9) {
231  double step_size = 1.;
232  while (true) {
233  const StateType next_estimate =
235  const StateType next_error =
236  ComputeWeightedError(next_estimate, states);
237  if (next_error.norm() < weighted_error.norm()) {
238  current_estimate = next_estimate;
239  weighted_error = next_error;
240  break;
241  }
242  step_size *= 0.5;
243  CHECK_GT(step_size, 1e-3) << "Step size too small, line search failed.";
244  }
245  ++iterations;
246  CHECK_LT(iterations, 20) << "Too many iterations.";
247  }
248  return current_estimate;
249  }
250
251  // According to Wikipedia these are the normal values. Thrun does not
252  // mention those.
253  constexpr static FloatType kAlpha = 1e-3;
254  constexpr static FloatType kKappa = 0.;
255  constexpr static FloatType kBeta = 2.;
256  constexpr static FloatType kLambda = sqr(kAlpha) * (N + kKappa) - N;
257  constexpr static FloatType kMeanWeight0 = kLambda / (N + kLambda);
258  constexpr static FloatType kCovWeight0 =
259  kLambda / (N + kLambda) + (1. - sqr(kAlpha) + kBeta);
260  constexpr static FloatType kMeanWeightI = 1. / (2. * (N + kLambda));
261  constexpr static FloatType kCovWeightI = kMeanWeightI;
262
264  const std::function<StateType(const StateType& state, const StateType& delta)>
266  const std::function<StateType(const StateType& origin,
267  const StateType& target)>
269 };
270
271 template <typename FloatType, int N>
273 template <typename FloatType, int N>
275 template <typename FloatType, int N>
277 template <typename FloatType, int N>
279 template <typename FloatType, int N>
281 template <typename FloatType, int N>
283 template <typename FloatType, int N>
285 template <typename FloatType, int N>
287
288 } // namespace kalman_filter
289 } // namespace cartographer
290
291 #endif // CARTOGRAPHER_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_
StateType ComputeWeightedError(const StateType &mean_estimate, const std::vector< StateType > &states)
void Observe(std::function< Eigen::Matrix< FloatType, K, 1 >(const StateType &)> h, const GaussianDistribution< FloatType, K > &delta)
StateType ComputeMean(const std::vector< StateType > &states)
const std::function< StateType(const StateType &origin, const StateType &target)> compute_delta_
void CheckSymmetric(const Eigen::Matrix< FloatType, N, N > &A)
const Eigen::Matrix< T, N, 1 > & GetMean() const
Eigen::Matrix< FloatType, N, N > MatrixSqrt(const Eigen::Matrix< FloatType, N, N > &A)
const std::function< StateType(const StateType &state, const StateType &delta)> add_delta_
void Predict(std::function< StateType(const StateType &)> g, const GaussianDistribution< FloatType, N > &epsilon)
Eigen::Matrix< FloatType, N, N > OuterProduct(const Eigen::Matrix< FloatType, N, 1 > &v)
const GaussianDistribution< FloatType, N > & GetBelief() const
UnscentedKalmanFilter(const GaussianDistribution< FloatType, N > &initial_belief, std::function< StateType(const StateType &state, const StateType &delta)> add_delta=[](const StateType &state, const StateType &delta){return state+delta;}, std::function< StateType(const StateType &origin, const StateType &target)> compute_delta=[](const StateType &origin, const StateType &target){return target-origin;})
const Eigen::Matrix< T, N, N > & GetCovariance() const
constexpr FloatType sqr(FloatType a)

cartographer
Author(s):
autogenerated on Mon Jun 10 2019 12:51:39