Apollo 10.0
自动驾驶开放平台
kalman_filter.h
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2017 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
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
22#pragma once
23
24#include <sstream>
25#include <string>
26
27#include "Eigen/Dense"
28
29#include "cyber/common/log.h"
31
36namespace apollo {
37namespace common {
38namespace math {
39
49template <typename T, unsigned int XN, unsigned int ZN, unsigned int UN>
51 public:
58 F_.setIdentity();
59 Q_.setZero();
60 H_.setIdentity();
61 R_.setZero();
62 B_.setZero();
63
64 x_.setZero();
65 P_.setZero();
66 y_.setZero();
67 S_.setZero();
68 K_.setZero();
69 }
70
77 void SetStateEstimate(const Eigen::Matrix<T, XN, 1> &x,
78 const Eigen::Matrix<T, XN, XN> &P) {
79 x_ = x;
80 P_ = P;
81 is_initialized_ = true;
82 }
83
89 KalmanFilter(const Eigen::Matrix<T, XN, 1> &x,
90 const Eigen::Matrix<T, XN, XN> &P)
91 : KalmanFilter() {
92 SetStateEstimate(x, P);
93 }
94
98 virtual ~KalmanFilter() {}
99
105 void SetTransitionMatrix(const Eigen::Matrix<T, XN, XN> &F) { F_ = F; }
106
112 void SetTransitionNoise(const Eigen::Matrix<T, XN, XN> &Q) { Q_ = Q; }
113
119 void SetObservationMatrix(const Eigen::Matrix<T, ZN, XN> &H) { H_ = H; }
120
126 void SetObservationNoise(const Eigen::Matrix<T, ZN, ZN> &R) { R_ = R; }
127
133 void SetStateCovariance(const Eigen::Matrix<T, XN, XN> &P) { P_ = P; }
134
140 void SetControlMatrix(const Eigen::Matrix<T, XN, UN> &B) { B_ = B; }
141
147 const Eigen::Matrix<T, XN, XN> &GetTransitionMatrix() const { return F_; }
148
154 const Eigen::Matrix<T, XN, XN> &GetTransitionNoise() const { return Q_; }
155
161 const Eigen::Matrix<T, ZN, XN> &GetObservationMatrix() const { return H_; }
162
168 const Eigen::Matrix<T, ZN, ZN> &GetObservationNoise() const { return R_; }
169
175 const Eigen::Matrix<T, XN, UN> &GetControlMatrix() const { return B_; }
176
182 void Predict(
183 const Eigen::Matrix<T, UN, 1> &u = Eigen::Matrix<T, UN, 1>::Zero());
184
190 void Correct(const Eigen::Matrix<T, ZN, 1> &z);
191
197 Eigen::Matrix<T, XN, 1> GetStateEstimate() const { return x_; }
198
204 Eigen::Matrix<T, XN, XN> GetStateCovariance() const { return P_; }
205
211 std::string DebugString() const;
212
217 bool IsInitialized() const { return is_initialized_; }
218
219 private:
220 // Mean of current state belief distribution
221 Eigen::Matrix<T, XN, 1> x_;
222
223 // Covariance of current state belief dist
224 Eigen::Matrix<T, XN, XN> P_;
225
226 // State transition matrix under zero control
227 Eigen::Matrix<T, XN, XN> F_;
228
229 // Covariance of the state transition noise
230 Eigen::Matrix<T, XN, XN> Q_;
231
232 // Observation matrix
233 Eigen::Matrix<T, ZN, XN> H_;
234
235 // Covariance of observation noise
236 Eigen::Matrix<T, ZN, ZN> R_;
237
238 // Control matrix in state transition rule
239 Eigen::Matrix<T, XN, UN> B_;
240
241 // Innovation; marked as member to prevent memory re-allocation.
242 Eigen::Matrix<T, ZN, 1> y_;
243
244 // Innovation covariance; marked as member to prevent memory re-allocation.
245 Eigen::Matrix<T, ZN, ZN> S_;
246
247 // Kalman gain; marked as member to prevent memory re-allocation.
248 Eigen::Matrix<T, XN, ZN> K_;
249
250 // true iff SetStateEstimate has been called.
251 bool is_initialized_ = false;
252};
253
254template <typename T, unsigned int XN, unsigned int ZN, unsigned int UN>
256 const Eigen::Matrix<T, UN, 1> &u) {
257 ACHECK(is_initialized_);
258
259 x_ = F_ * x_ + B_ * u;
260
261 P_ = F_ * P_ * F_.transpose() + Q_;
262}
263
264template <typename T, unsigned int XN, unsigned int ZN, unsigned int UN>
266 const Eigen::Matrix<T, ZN, 1> &z) {
267 ACHECK(is_initialized_);
268 y_ = z - H_ * x_;
269
270 S_ = static_cast<Eigen::Matrix<T, ZN, ZN>>(H_ * P_ * H_.transpose() + R_);
271
272 K_ = static_cast<Eigen::Matrix<T, XN, ZN>>(P_ * H_.transpose() *
273 PseudoInverse<T, ZN>(S_));
274
275 x_ = x_ + K_ * y_;
276
277 P_ = static_cast<Eigen::Matrix<T, XN, XN>>(
278 (Eigen::Matrix<T, XN, XN>::Identity() - K_ * H_) * P_);
279}
280
281template <typename T, unsigned int XN, unsigned int ZN, unsigned int UN>
283 Eigen::IOFormat clean_fmt(4, 0, ", ", " ", "[", "]");
284 std::stringstream ss;
285 ss << "F = " << F_.format(clean_fmt) << "\n"
286 << "B = " << B_.format(clean_fmt) << "\n"
287 << "H = " << H_.format(clean_fmt) << "\n"
288 << "Q = " << Q_.format(clean_fmt) << "\n"
289 << "R = " << R_.format(clean_fmt) << "\n"
290 << "x = " << x_.format(clean_fmt) << "\n"
291 << "P = " << P_.format(clean_fmt) << "\n";
292 return ss.str();
293}
294
295} // namespace math
296} // namespace common
297} // namespace apollo
Implements a discrete-time Kalman filter.
KalmanFilter(const Eigen::Matrix< T, XN, 1 > &x, const Eigen::Matrix< T, XN, XN > &P)
Constructor which fully initializes the Kalman filter
std::string DebugString() const
Gets debug string containing detailed information about the filter.
void SetStateCovariance(const Eigen::Matrix< T, XN, XN > &P)
Changes the covariance matrix of current state belief distribution.
const Eigen::Matrix< T, ZN, ZN > & GetObservationNoise() const
Get the covariance matrix of the observation noise.
void SetTransitionMatrix(const Eigen::Matrix< T, XN, XN > &F)
Changes the system transition function under zero control.
void Predict(const Eigen::Matrix< T, UN, 1 > &u=Eigen::Matrix< T, UN, 1 >::Zero())
Updates the state belief distribution given the control input u.
KalmanFilter()
Constructor which defers initialization until the initial state distribution parameters are set (with...
void SetTransitionNoise(const Eigen::Matrix< T, XN, XN > &Q)
Changes the covariance matrix of the transition noise.
Eigen::Matrix< T, XN, 1 > GetStateEstimate() const
Gets mean of our current state belief distribution
bool IsInitialized() const
Get initialization state of the filter
const Eigen::Matrix< T, ZN, XN > & GetObservationMatrix() const
Get the observation matrix, which maps states to observations.
void SetObservationNoise(const Eigen::Matrix< T, ZN, ZN > &R)
Changes the covariance matrix of the observation noise.
void SetStateEstimate(const Eigen::Matrix< T, XN, 1 > &x, const Eigen::Matrix< T, XN, XN > &P)
Sets the initial state belief distribution.
const Eigen::Matrix< T, XN, UN > & GetControlMatrix() const
Get the control matrix in the state transition rule.
void SetControlMatrix(const Eigen::Matrix< T, XN, UN > &B)
Changes the control matrix in the state transition rule.
Eigen::Matrix< T, XN, XN > GetStateCovariance() const
Gets covariance of our current state belief distribution
void Correct(const Eigen::Matrix< T, ZN, 1 > &z)
Updates the state belief distribution given an observation z.
const Eigen::Matrix< T, XN, XN > & GetTransitionNoise() const
Get the covariance matrix of the transition noise.
const Eigen::Matrix< T, XN, XN > & GetTransitionMatrix() const
Get the system transition function under zero control.
void SetObservationMatrix(const Eigen::Matrix< T, ZN, XN > &H)
Changes the observation matrix, which maps states to observations.
#define ACHECK(cond)
Definition log.h:80
Defines some useful matrix operations.
class register implement
Definition arena_queue.h:37