Lodestar
An integrated real-time control package in C++
ContinuousSampledEKF.hpp
1 //
2 // Created by Hamza El-Kebir on 6/25/21.
3 //
4 
5 #ifndef LODESTAR_CONTINUOUSSAMPLEDEKF_HPP
6 #define LODESTAR_CONTINUOUSSAMPLEDEKF_HPP
7 
8 #include <Eigen/Dense>
9 #include <Eigen/QR>
10 #include <functional>
11 #include <type_traits>
12 
13 #include "Lodestar/primitives/integrators/IntegratorsEnum.hpp"
14 #include "Lodestar/primitives/integrators/BogackiShampine.hpp"
15 #include "Lodestar/primitives/integrators/RungeKuttaFehlberg45.hpp"
16 #include "Lodestar/primitives/integrators/RungeKuttaFehlberg78.hpp"
17 
18 #include <iostream>
19 
20 namespace ls {
21  namespace filter {
22  class ContinuousSampledEKF {
23  public:
24  using integrators = ls::primitives::IntegratorsEnum;
25 
26  template<ls::primitives::IntegratorsEnum TIntegrator = ls::primitives::IntegratorsEnum::RungeKuttaFehlberg45, typename TScalar = double, int TStateDim = Eigen::Dynamic, int TInputDim = Eigen::Dynamic, int TOutputDim = Eigen::Dynamic>
27  struct EKFParameters {
28  const ls::primitives::IntegratorsEnum kIntegrator = TIntegrator;
29  using scalar_type = TScalar;
30  const int kStateDim = TStateDim;
31  const int kInputDim = TInputDim;
32  const int kOutputDim = TOutputDim;
33 
34  typedef Eigen::Matrix<TScalar, TStateDim, TStateDim> TDStateMatrix;
35  typedef Eigen::Matrix<TScalar, TStateDim, 1> TDStateVector;
36  typedef Eigen::Matrix<TScalar, TStateDim, TInputDim> TDInputMatrix;
37  typedef Eigen::Matrix<TScalar, TInputDim, 1> TDInputVector;
38  typedef Eigen::Matrix<TScalar, TOutputDim, TStateDim> TDOutputMatrix;
39  typedef Eigen::Matrix<TScalar, TOutputDim, 1> TDOutputVector;
40  typedef Eigen::Matrix<TScalar, TOutputDim, TInputDim> TDFeedforwardMatrix;
41 
42  typedef Eigen::Matrix<TScalar, TStateDim, TOutputDim> TDKalmanGain;
43 
44  typedef TDStateMatrix TDProcessCovarianceMatrix;
45  typedef Eigen::Matrix<TScalar, TOutputDim, TOutputDim> TDObservationCovarianceMatrix;
46 
47  typedef ls::primitives::IntegratorsEnum TDIntegratorsEnum;
48 
49  typedef std::function<TDStateVector(TScalar, const TDStateVector &, const TDInputVector &)> TDOde;
50  typedef std::function<TDStateMatrix(TScalar, const TDStateMatrix &, const TDStateVector &,
51  const TDInputVector &)> TDPOde;
52  typedef std::function<TDStateMatrix(const TDStateVector &, const TDInputVector &)> TDStateJacobian;
53  typedef std::function<TDOutputMatrix(const TDStateVector &)> TDOutputJacobian;
54  typedef std::function<TDOutputVector(const TDStateVector &)> TDOutputMap;
55 
56  typedef std::function<TDStateVector(TScalar, const TDStateVector &)> TDOdeFunc;
57  typedef std::function<TDStateMatrix(TScalar, const TDStateMatrix &)> TDMatrixOdeFunc;
58 
59  void initEquations(const TDOde &ordinaryDiffEq, const TDStateJacobian &stateJacobianEq,
60  const TDOutputJacobian &outputJacobianEq, const TDOutputMap &outputMapEq)
61  {
62  ode = ordinaryDiffEq;
63  stateJacobian = stateJacobianEq;
64  outputJacobian = outputJacobianEq;
65  outputMap = outputMapEq;
66 
67  Pdot = [&](TScalar t, const TDStateMatrix &P, const TDStateVector &x, const TDInputVector &u) {
68  const auto F = stateJacobian(x, u);
69  std::cout << "F at t = " << t << ": " << F << std::endl;
70  std::cout << "P at t = " << t << ": " << P << std::endl;
71  std::cout << "Pdot at t = " << t << ": " << F * P + P * F.transpose() + Q << std::endl;
72 
73  // NOTE: The return value must be explicitly cast as type TDStateMatrix, otherwise the copy
74  // constructor in Eigen fails.
75  TDStateMatrix ret;
76  ret = F * P + P * F.transpose() + Q;
77 
78  return ret;
79  };
80  }
81 
82  void
83  initParams(const TDStateVector &x0, const TDStateMatrix &P0 = TDStateMatrix::Zero(), TScalar t0 = 0)
84  {
85  xhat = x0;
86  P = P0;
87  time = t0;
88  }
89 
90  TDOde ode;
91  TDStateJacobian stateJacobian;
92  TDOutputJacobian outputJacobian;
93  TDOutputMap outputMap;
94 
95  TDProcessCovarianceMatrix Q;
96  TDObservationCovarianceMatrix R;
97  TDStateMatrix P;
98 
99  TDStateVector xhat;
100  TScalar time;
101 
102  TDPOde Pdot;
103 
104  // TODO: Add template parameter for integration scheme;
105  // Add initialization and prediction routines;
106  // Add time member variable;
107  // Implement unit tests.
108 
109  template<TDIntegratorsEnum T_TIntegrator = TIntegrator>
110  typename std::enable_if<T_TIntegrator == TDIntegratorsEnum::BogackiShampine, void>::type
111  predict(TDStateVector &xout, TDStateMatrix &Pout, const TDStateVector &x, const TDInputVector &u,
112  const TScalar h)
113  {
114  xout = x;
115  auto f = std::bind(ode, std::placeholders::_1, std::placeholders::_2, u);
116 
117  // FIXME: Make x time-dependent.
118  Pout = P;
119  auto F = std::bind(Pdot, std::placeholders::_1, std::placeholders::_2, x, u);
120  TScalar t = 0;
121 
122  ls::primitives::BogackiShampine<TDStateVector, TScalar>::integrateSimple(f, t, xout, h, 1);
123 
124  t = 0;
125  ls::primitives::BogackiShampine<TDStateMatrix, TScalar>::integrateSimple(F, t, Pout, h, 1);
126  }
127 
128  template<TDIntegratorsEnum T_TIntegrator = TIntegrator>
129  typename std::enable_if<T_TIntegrator == TDIntegratorsEnum::RungeKuttaFehlberg45, void>::type
130  predict(TDStateVector &xout, TDStateMatrix &Pout, const TDStateVector &x, const TDInputVector &u,
131  const TScalar h)
132  {
133  xout = x;
134  auto f = std::bind(ode, std::placeholders::_1, std::placeholders::_2, u);
135 
136  // FIXME: Make x time-dependent.
137  Pout = P;
138  auto F = std::bind(Pdot, std::placeholders::_1, std::placeholders::_2, x, u);
139  TScalar t = 0;
140 
141  ls::primitives::RungeKuttaFehlberg45<TDStateVector, TScalar>::integrateSimple(f, t, xout, h, 1);
142 
143  t = 0;
144  ls::primitives::RungeKuttaFehlberg45<TDStateMatrix, TScalar>::integrateSimple(F, t, Pout, h, 1);
145  }
146 
147  template<TDIntegratorsEnum T_TIntegrator = TIntegrator>
148  typename std::enable_if<T_TIntegrator == TDIntegratorsEnum::RungeKuttaFehlberg78, void>::type
149  predict(TDStateVector &xout, TDStateMatrix &Pout, const TDStateVector &x, const TDInputVector &u,
150  const TScalar h)
151  {
152  xout = x;
153  auto f = std::bind(ode, std::placeholders::_1, std::placeholders::_2, u);
154  std::function<TDStateVector(TScalar, const TDStateVector &)> func = [&](TScalar t, const TDStateVector &x)
155  {
156  return ode(t, x, u);
157  };
158 
159  // FIXME: Make x time-dependent.
160  Pout = P;
161  auto F = std::bind(Pdot, std::placeholders::_1, std::placeholders::_2, x, u);
162  std::function<TDStateMatrix (TScalar, const TDStateMatrix &)> Func = [&](TScalar t, const TDStateMatrix &P)
163  {
164  return Pdot(t, P, x, u);
165  };
166  TScalar t = 0;
167 
168  ls::primitives::RungeKuttaFehlberg78<TDStateVector, TScalar>::integrateSimple(func, t, xout, h, 1);
169 
170  t = 0;
171  ls::primitives::RungeKuttaFehlberg78<TDStateMatrix, TScalar>::integrateSimple(Func, t, Pout, h, 1);
172  }
173 
174  void update(TDStateVector &x, const TDOutputVector &z)
175  {
176  const TDOutputMatrix H = outputJacobian(x);
177  const TDKalmanGain K = P * H.transpose() * (H * P * H.transpose() + R).completeOrthogonalDecomposition().pseudoInverse();
178  x = x + K * (z - outputMap(x));
179  P = (TDStateMatrix::Identity() - K * H) * P;
180  }
181 
182  void step(const TDInputVector &u, const TDOutputVector &z, const TScalar h)
183  {
184  TDStateVector xnew;
185  TDStateMatrix Pnew;
186 
187  predict(xnew, Pnew, xhat, u, h);
188  xhat = xnew;
189  P = Pnew;
190 
191  std::cout << "xnew " << xnew << std::endl;
192  std::cout << "Pnew " << Pnew << std::endl;
193 
194  update(xhat, z);
195 
196  time += h;
197  }
198 
199  void setCovarianceMatrices(const TDProcessCovarianceMatrix &QMatrix, const TDObservationCovarianceMatrix &RMatrix)
200  {
201  Q = QMatrix;
202  R = RMatrix;
203  }
204 
205  const TDStateVector &getStateEstimate() const
206  {
207  return xhat;
208  }
209 
210  TScalar getCurrentTime() const
211  {
212  return time;
213  }
214  };
215  };
216  }
217 }
218 
219 
220 #endif //LODESTAR_CONTINUOUSSAMPLEDEKF_HPP
ls::filter::ContinuousSampledEKF::EKFParameters::TDPOde
std::function< TDStateMatrix(TScalar, const TDStateMatrix &, const TDStateVector &, const TDInputVector &)> TDPOde
Ordinary differential equation: f(t, x, u)
Definition: ContinuousSampledEKF.hpp:51
ls::filter::ContinuousSampledEKF::EKFParameters::TDOdeFunc
std::function< TDStateVector(TScalar, const TDStateVector &)> TDOdeFunc
Output map: f(x)
Definition: ContinuousSampledEKF.hpp:56
ls::filter::ContinuousSampledEKF
Definition: ContinuousSampledEKF.hpp:22
ls
Main Lodestar code.
Definition: BilinearTransformation.hpp:12
ls::filter::ContinuousSampledEKF::EKFParameters::TDOutputMap
std::function< TDOutputVector(const TDStateVector &)> TDOutputMap
Jacobian output matrix: f(x)
Definition: ContinuousSampledEKF.hpp:54
ls::primitives::RungeKuttaFehlberg45::integrateSimple
static void integrateSimple(const TDFunction &f, TScalarType &t, TType &y, TScalarType h, size_t N=1)
Simple integration using the fifth-order scheme in the Runge-Kutta-Fehlberg method.
Definition: RungeKuttaFehlberg45.hpp:170
ls::primitives::RungeKuttaFehlberg78::integrateSimple
static void integrateSimple(const TDFunction &f, TScalarType &t, TType &y, TScalarType h, size_t N=1)
Simple integration using the eighth-order scheme in the Runge-Kutta-Fehlberg method.
Definition: RungeKuttaFehlberg78.hpp:347
ls::filter::ContinuousSampledEKF::EKFParameters
Definition: ContinuousSampledEKF.hpp:27
ls::primitives::BogackiShampine::integrateSimple
static void integrateSimple(const TDFunction &f, TScalarType &t, TType &y, TScalarType h, size_t N=1)
Simple integration using the third-order scheme in the Bogacki-Shampine method.
Definition: BogackiShampine.hpp:131
ls::filter::ContinuousSampledEKF::EKFParameters::TDStateJacobian
std::function< TDStateMatrix(const TDStateVector &, const TDInputVector &)> TDStateJacobian
Matrix ordinary differential equation: f(t, P, x, u)
Definition: ContinuousSampledEKF.hpp:52
ls::filter::ContinuousSampledEKF::EKFParameters::TDOutputJacobian
std::function< TDOutputMatrix(const TDStateVector &)> TDOutputJacobian
Jacobian state matrix: f(x, u)
Definition: ContinuousSampledEKF.hpp:53