ethzasl-msf - Modular Sensor Fusion
Time delay compensated single and multi sensor fusion framework based on an EKF
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
msf_measurement.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2013 Simon Lynen, ASL, ETH Zurich, Switzerland
3  * You can contact the author at <slynen at ethz dot ch>
4  *
5  * Licensed under the Apache License, Version 2.0 (the "License");
6  * you may not use this file except in compliance with the License.
7  * You may obtain a copy of the License at
8  *
9  * http://www.apache.org/licenses/LICENSE-2.0
10  *
11  * Unless required by applicable law or agreed to in writing, software
12  * distributed under the License is distributed on an "AS IS" BASIS,
13  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14  * See the License for the specific language governing permissions and
15  * limitations under the License.
16  */
17 #include <msf_core/msf_core.h>
18 
19 namespace msf_core {
20 template<typename EKFState_T>
21 MSF_MeasurementBase<EKFState_T>::MSF_MeasurementBase(bool isabsoluteMeasurement,
22  int sensorID)
23  : sensorID_(sensorID),
24  isabsolute_(isabsoluteMeasurement),
25  time(0) {
26 }
27 
28 template<typename EKFState_T>
29 template<class H_type, class Res_type, class R_type>
30 void MSF_MeasurementBase<EKFState_T>::calculateAndApplyCorrection(
31  shared_ptr<EKFState_T> state, MSF_Core<EKFState_T>& core,
32  const Eigen::MatrixBase<H_type>& H_delayed,
33  const Eigen::MatrixBase<Res_type> & res_delayed,
34  const Eigen::MatrixBase<R_type>& R_delayed) {
35 
36  EIGEN_STATIC_ASSERT_FIXED_SIZE (H_type);
37  EIGEN_STATIC_ASSERT_FIXED_SIZE (R_type);
38 
39  // Get measurements.
41  Eigen::Matrix<double, MSF_Core<EKFState_T>::nErrorStatesAtCompileTime, 1> correction_;
42 
43  R_type S;
44  Eigen::Matrix<double, MSF_Core<EKFState_T>::nErrorStatesAtCompileTime,
45  R_type::RowsAtCompileTime> K;
46  typename MSF_Core<EKFState_T>::ErrorStateCov & P = state->P;
47 
48  S = H_delayed * P * H_delayed.transpose() + R_delayed;
49  K = P * H_delayed.transpose() * S.inverse();
50 
51  correction_ = K * res_delayed;
52  const typename MSF_Core<EKFState_T>::ErrorStateCov KH =
53  (MSF_Core<EKFState_T>::ErrorStateCov::Identity() - K * H_delayed);
54  P = KH * P * KH.transpose() + K * R_delayed * K.transpose();
55 
56  // Make sure P stays symmetric.
57  P = 0.5 * (P + P.transpose());
58 
59  core.applyCorrection(state, correction_);
60 }
61 
62 template<typename EKFState_T>
63 void MSF_MeasurementBase<EKFState_T>::calculateAndApplyCorrection(
64  shared_ptr<EKFState_T> state, MSF_Core<EKFState_T>& core,
65  const Eigen::MatrixXd& H_delayed, const Eigen::MatrixXd & res_delayed,
66  const Eigen::MatrixXd& R_delayed) {
67 
68  // Get measurements.
70  Eigen::Matrix<double, MSF_Core<EKFState_T>::nErrorStatesAtCompileTime, 1> correction_;
71 
72  Eigen::MatrixXd S;
73  Eigen::MatrixXd K(
74  static_cast<int>(MSF_Core<EKFState_T>::nErrorStatesAtCompileTime),
75  R_delayed.rows());
76  typename MSF_Core<EKFState_T>::ErrorStateCov & P = state->P;
77 
78  S = H_delayed * P * H_delayed.transpose() + R_delayed;
79  K = P * H_delayed.transpose() * S.inverse();
80 
81  correction_ = K * res_delayed;
82  const typename MSF_Core<EKFState_T>::ErrorStateCov KH =
83  (MSF_Core<EKFState_T>::ErrorStateCov::Identity() - K * H_delayed);
84  P = KH * P * KH.transpose() + K * R_delayed * K.transpose();
85 
86  // Make sure P stays symmetric.
87  P = 0.5 * (P + P.transpose());
88 
89  core.applyCorrection(state, correction_);
90 }
91 
92 template<typename EKFState_T>
93 template<class H_type, class Res_type, class R_type>
94 void MSF_MeasurementBase<EKFState_T>::calculateAndApplyCorrectionRelative(
95  shared_ptr<EKFState_T> state_old, shared_ptr<EKFState_T> state_new,
96  MSF_Core<EKFState_T>& core, const Eigen::MatrixBase<H_type>& H_old,
97  const Eigen::MatrixBase<H_type>& H_new,
98  const Eigen::MatrixBase<Res_type> & res,
99  const Eigen::MatrixBase<R_type>& R) {
100 
101  EIGEN_STATIC_ASSERT_FIXED_SIZE (H_type);
102  EIGEN_STATIC_ASSERT_FIXED_SIZE (R_type);
103 
104  // Get measurements.
106  Eigen::Matrix<double, MSF_Core<EKFState_T>::nErrorStatesAtCompileTime, 1> correction_;
107 
108  enum {
109  Pdim = MSF_Core<EKFState_T>::nErrorStatesAtCompileTime
110  };
111 
112  // Get the accumulated system dynamics.
113  Eigen::Matrix<double, Pdim, Pdim> F_accum;
114  core.getAccumF_SC(state_old, state_new, F_accum);
115 
116  /*[
117  * | P_kk P_kk * F' |
118  * P_SC = | |
119  * | F * P_kk P_mk |
120  */
121  Eigen::Matrix<double, 2 * Pdim, 2 * Pdim> P_SC;
122  P_SC.template block<Pdim, Pdim>(0, 0) = state_old->P;
123  // According to TRO paper, ICRA paper has a mistake here.
124  P_SC.template block<Pdim, Pdim>(0, Pdim) = state_old->P * F_accum.transpose();
125  P_SC.template block<Pdim, Pdim>(Pdim, 0) = F_accum * state_old->P;
126  P_SC.template block<Pdim, Pdim>(Pdim, Pdim) = state_new->P;
127 
128  /*
129  * H_SC = [H_kk H_mk]
130  */
131  Eigen::Matrix<double, H_type::RowsAtCompileTime, H_type::ColsAtCompileTime * 2> H_SC;
132 
133  H_SC.template block<H_type::RowsAtCompileTime, H_type::ColsAtCompileTime>(0,
134  0) =
135  H_old;
136  H_SC.template block<H_type::RowsAtCompileTime, H_type::ColsAtCompileTime>(
137  0, H_type::ColsAtCompileTime) = H_new;
138 
139  R_type S_SC;
140  S_SC = H_SC * P_SC * H_SC.transpose() + R;
141 
142  //TODO (slynen): Only compute the part of K_SC that we need.
143  Eigen::Matrix<double, MSF_Core<EKFState_T>::nErrorStatesAtCompileTime * 2,
144  R_type::RowsAtCompileTime> K_SC;
145  K_SC = P_SC * H_SC.transpose() * S_SC.inverse();
146 
147  Eigen::Matrix<double, MSF_Core<EKFState_T>::nErrorStatesAtCompileTime,
148  R_type::RowsAtCompileTime> K;
149  K = K_SC
150  .template block<MSF_Core<EKFState_T>::nErrorStatesAtCompileTime,
151  R_type::RowsAtCompileTime>(
152  MSF_Core<EKFState_T>::nErrorStatesAtCompileTime, 0);
153 
154  correction_ = K * res;
155 
156  typename MSF_Core<EKFState_T>::ErrorStateCov & P = state_new->P;
157  P = P - K * S_SC * K.transpose();
158 
159  // Make sure P stays symmetric.
160  // TODO (slynen): EV, set Evalues<eps to zero, then reconstruct.
161  state_new->P = 0.5 * (state_new->P + state_new->P.transpose());
162 
163  core.applyCorrection(state_new, correction_);
164 }
165 
166 template<typename EKFState_T>
167 void MSF_InitMeasurement<EKFState_T>::apply(
168  shared_ptr<EKFState_T> stateWithCovariance, MSF_Core<EKFState_T>& core) {
169 
170  // Makes this state a valid starting point.
171  stateWithCovariance->time = this->time;
172 
173  boost::fusion::for_each(stateWithCovariance->statevars,
174  msf_tmp::copyInitStates<EKFState_T>(InitState));
175 
176  if (!(InitState.P.minCoeff() == 0 && InitState.P.maxCoeff() == 0)) {
177  stateWithCovariance->P = InitState.P;
178  MSF_WARN_STREAM("Using user defined initial error state covariance.");
179  } else {
180  MSF_WARN_STREAM(
181  "Using simulated core plus fixed diag initial error state covariance.");
182  }
183 
184  if (ContainsInitialSensorReadings_) {
185  stateWithCovariance->a_m = InitState.a_m;
186  stateWithCovariance->w_m = InitState.w_m;
187  } else {
188  stateWithCovariance->a_m.setZero();
189  stateWithCovariance->w_m.setZero();
190  }
191 
192  core.usercalc().publishStateInitial(stateWithCovariance);
193 
194 }
195 }
Generated on Wed Nov 6 2013 22:21:53 for ethzasl-msf - Modular Sensor Fusion by   doxygen 1.8.1.2