iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
UKFImpl.H
Go to the documentation of this file.
1 /*! @file
2  @author Randolph Voorhies (voorhies at usc dot edu)
3  @copyright GNU Public License (GPL v3)
4  @section License
5  @verbatim
6  // ////////////////////////////////////////////////////////////////////////
7  // The iLab Neuromorphic Robotics Toolkit (NRT) //
8  // Copyright 2010-2012 by the University of Southern California (USC) //
9  // and the iLab at USC. //
10  // //
11  // iLab - University of Southern California //
12  // Hedco Neurociences Building, Room HNB-10 //
13  // Los Angeles, Ca 90089-2520 - USA //
14  // //
15  // See http://ilab.usc.edu for information about this project. //
16  // ////////////////////////////////////////////////////////////////////////
17  // This file is part of The iLab Neuromorphic Robotics Toolkit. //
18  // //
19  // The iLab Neuromorphic Robotics Toolkit is free software: you can //
20  // redistribute it and/or modify it under the terms of the GNU General //
21  // Public License as published by the Free Software Foundation, either //
22  // version 3 of the License, or (at your option) any later version. //
23  // //
24  // The iLab Neuromorphic Robotics Toolkit is distributed in the hope //
25  // that it will be useful, but WITHOUT ANY WARRANTY; without even the //
26  // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR //
27  // PURPOSE. See the GNU General Public License for more details. //
28  // //
29  // You should have received a copy of the GNU General Public License //
30  // along with The iLab Neuromorphic Robotics Toolkit. If not, see //
31  // <http://www.gnu.org/licenses/>. //
32  // ////////////////////////////////////////////////////////////////////////
33  @endverbatim */
34 
35 
36 
37 // ######################################################################
38 template <class StateDefinitionType, class ObservationDefinitionType, class FilterModel>
40  nrt::GaussianPDF<StateDefinitionType> const & initialState,
41  Eigen::Matrix<double, itsNS, itsNS> const & predictionCovariance,
42  double alpha, double k, double beta) :
43  itsState(initialState),
44  itsR(predictionCovariance)
45 {
46  static_assert(std::is_base_of<StateDefinitionBase, StateDefinitionType>::value,
47  "StateDefinitionType must be a nrt::StateDefinition");
48 
49  static_assert(std::is_base_of<StateDefinitionBase, ObservationDefinitionType>::value,
50  "ObservationDefinitionType must be a nrt::StateDefinition");
51 
52  // Calculate Gamma
53  double lambda = alpha*alpha * (itsNS + k) - itsNS;
54  itsGamma = sqrt(itsNS + lambda);
55 
56  itsWm = std::vector<double>(itsNS*2 + 1, 1.0 / (2.0*(itsNS+lambda)));
57  itsWm.at(0) = lambda / (itsNS+lambda);
58 
59  itsWc = std::vector<double>(itsNS*2 + 1, 1.0 / (2.0*(itsNS+lambda)));
60  itsWc.at(0) = lambda / (itsNS+lambda) + (1 - alpha*alpha + beta);
61 }
62 
63 // ######################################################################
64 template <class StateDefinitionType, class ObservationDefinitionType, class FilterModel>
67 {
68  return itsState;
69 }
70 
71 // ######################################################################
72 template <class StateDefinitionType, class ObservationDefinitionType, class FilterModel>
74 {
75  // Generate the sigma points
76  std::vector<Eigen::Matrix<double, itsNS, 1>> sigmaPoints = generateSigmaPoints();
77 
78  // Compute a next-step prediction for each sigma point with the user's 'predict' function
79  std::vector<Eigen::Matrix<double, itsNS, 1>> predictions;
80  for(StateVectorType const & Xi : sigmaPoints)
81  predictions.push_back(FilterModel::predict(Xi, control).template access<>());
82 
83  // Compute the new estimated mean as the weighted average of the predicted states
84  Eigen::Matrix<double, itsNS, 1> newMean = Eigen::Matrix<double, itsNS, 1>::Zero();
85  for(size_t i=0; i<sigmaPoints.size(); ++i)
86  newMean += itsWm.at(i) * predictions.at(i);
87 
88  // Compute the new covariance
89  Eigen::Matrix<double, itsNS, itsNS> newCovariance = itsR;
90  for(size_t i=0; i<predictions.size(); ++i)
91  {
92  Eigen::Matrix<double, itsNS, 1> Xiu = predictions.at(i) - newMean;
93  newCovariance += itsWc.at(i) * (Xiu * Xiu.transpose());
94  }
95 
96  // Replace the old state with the new
97  itsState = nrt::GaussianPDF<StateDefinitionType>(newMean, newCovariance);
98 }
99 
100 // ######################################################################
101 template <class StateDefinitionType, class ObservationDefinitionType, class FilterModel>
103 {
104  // Generate the sigma points
105  std::vector<Eigen::Matrix<double, itsNS, 1>> sigmaPoints = generateSigmaPoints();
106 
107  // Compute the expected observation for each of the sigma points
108  std::vector<Eigen::Matrix<double, itsNO, 1>> observations;
109  for(StateVectorType const & Xi : sigmaPoints)
110  observations.push_back(FilterModel::observe(Xi).template access<>());
111 
112  // Compute the weighted mean of the expected observations
113  Eigen::Matrix<double, itsNO, 1> z_hat = Eigen::Matrix<double, itsNO, 1>::Zero();
114  for(size_t i=0; i<observations.size(); ++i)
115  z_hat += itsWm.at(i) * observations.at(i);
116 
117  // Compute the uncertainty (S) in the observation
118  Eigen::Matrix<double, itsNO, itsNO> S = observation.template covariance<>();
119  for(size_t i=0; i<observations.size(); ++i)
120  {
121  Eigen::Matrix<double, itsNO, 1> Ziz = observations.at(i) - z_hat;
122  S += itsWc.at(i) * (Ziz * Ziz.transpose());
123  }
124 
125  // Compute the cross-covariance between the state and observation
126  Eigen::Matrix<double, itsNS, itsNO> crossCov = Eigen::Matrix<double, itsNS, itsNO>::Zero();
127  for(size_t i=0; i<observations.size(); ++i)
128  {
129  crossCov +=
130  itsWc.at(i) *
131  (sigmaPoints.at(i) - itsState.template mean<>()) *
132  (observations.at(i) - z_hat).transpose();
133  }
134 
135  // Make sure S stays symmetric
136  S = (S.transpose()+S)*0.5;
137 
138  // Check to make sure the S matrix is invertible
139  if(!Eigen::FullPivLU<decltype(S)>(S).isInvertible())
140  NRT_WARNING("Filter is now unstable!");
141 
142  // Compute the Kalman gain
143  Eigen::Matrix<double, itsNS, itsNO> K = crossCov * S.inverse();
144 
145  // Compute the new mean
146  Eigen::Matrix<double, itsNS, 1>
147  newMean = itsState.template mean<>() + K * (observation.template mean<>() - z_hat);
148 
149  // Compute the new covariance
150  Eigen::Matrix<double, itsNS, itsNS>
151  newCovariance = itsState.template covariance<>() - K * S * K.transpose();
152 
153  // Replace the old state with the new
154  itsState = nrt::GaussianPDF<StateDefinitionType>(newMean, newCovariance);
155 }
156 
157 // ######################################################################
158 template <class StateDefinitionType, class ObservationDefinitionType, class FilterModel>
160  Eigen::Matrix<double, itsNS, itsNS> const & predictionCovariance)
161 {
162  itsR = predictionCovariance;
163 }
164 
165 // ######################################################################
166 template <class StateDefinitionType, class ObservationDefinitionType, class FilterModel>
167 std::vector<Eigen::Matrix<double, nrt::UKF<StateDefinitionType, ObservationDefinitionType, FilterModel>::itsNS, 1>>
169 {
170  // Find the square root of the covariance (via Cholesky factorization)
171  Eigen::Matrix<double,itsNS,itsNS> sqrtCovariance =
172  Eigen::Matrix<double,itsNS,itsNS>(itsState.template covariance<>().llt().matrixL());
173 
174  // Find the offsets from the mean for all of the sigma points
175  Eigen::Matrix<double, itsNS, itsNS> sigmaOffsets = itsGamma * sqrtCovariance;
176 
177  // Generate the sigma points
178  std::vector<Eigen::Matrix<double, itsNS, 1>> sigmaPoints;
179  sigmaPoints.push_back(itsState.template mean<>());
180  for(size_t i = 0; i<itsNS; ++i)
181  sigmaPoints.push_back(sigmaPoints.at(0) + sigmaOffsets.template block<itsNS, 1>(0, i));
182  for(size_t i = 0; i<itsNS; ++i)
183  sigmaPoints.push_back(sigmaPoints.at(0) - sigmaOffsets.template block<itsNS, 1>(0, i));
184 
185  return sigmaPoints;
186 }