00001 #ifndef INCLUDE_NAVLIB_NAVMATH
00002 #define INCLUDE_NAVLIB_NAVMATH
00003
00004 #include <algorithm>
00005 #include <boost/numeric/ublas/vector.hpp>
00006 #include <boost/numeric/ublas/vector_proxy.hpp>
00007 #include <boost/numeric/ublas/matrix.hpp>
00008 #include <boost/numeric/ublas/triangular.hpp>
00009 #include <boost/numeric/ublas/lu.hpp>
00010 #include <boost/numeric/ublas/io.hpp>
00011 #include <boost/function.hpp>
00012
00013 namespace ublas = boost::numeric::ublas;
00014
00015 namespace navlib
00016 {
00017
00020 template <typename T>
00021 bool invert (const ublas::matrix<T>& M, ublas::matrix<T>& InvM)
00022 {
00023
00024 assert(M.size1() == M.size2());
00025 assert(M.size1() == InvM.size1());
00026 assert(M.size2() == InvM.size2());
00027
00028
00029 ublas::matrix<T> A(M);
00030
00031
00032 ublas::permutation_matrix<std::size_t> pivots(A.size1());
00033
00034
00035 int res = lu_factorize(A, pivots);
00036 if( res != 0 ) return false;
00037
00038
00039 double det = 1.0;
00040 for (std::size_t k=0; k < pivots.size(); k++)
00041 {
00042 if (pivots(k) != k)
00043 det *= -1.0;
00044 det *= A(k, k);
00045 }
00046 if (det > 1.0e20) return false;
00047
00048
00049 InvM.assign(ublas::identity_matrix<T>(A.size1()));
00050
00051
00052 try
00053 {
00054 lu_substitute(A, pivots, InvM);
00055 }
00056 catch (ublas::internal_logic)
00057 {
00058 std::cerr << "internal logic error" << std::endl;
00059 }
00060 return true;
00061 }
00062
00063 template <typename T>
00064 ublas::matrix<T> jacobian(boost::function< ublas::vector<T> (ublas::vector<T>) > f, ublas::vector<T> x, T tol)
00065 {
00066 ublas::matrix<T> J = ublas::zero_matrix<T>(f(x).size(), x.size());
00067 for (unsigned int k=0; k<x.size(); k++)
00068 {
00069 ublas::vector<T> dx = ublas::zero_vector<T>(x.size());
00070 dx[k] = static_cast<T>(0.5)*tol;
00071 ublas::column(J,k) = (f(x+dx) - f(x-dx))/tol;
00072 }
00073 return J;
00074 }
00075
00078 template <typename T = double>
00079 class ExtendedKalmanFilter
00080 {
00081 public:
00083 typedef boost::function< ublas::vector<T> (ublas::vector<T>) > PredictFun;
00085 typedef boost::function< ublas::vector<T> (ublas::vector<T>) > ObserveFun;
00086
00089 ExtendedKalmanFilter(ublas::vector<T> x0, T kP=1.0e-9)
00090 {
00091 _x = x0;
00092 _P = kP*ublas::identity_matrix<T>(x0.size());
00093 }
00094
00097 ExtendedKalmanFilter(ublas::vector<T> x0, ublas::matrix<T> P0)
00098 {
00099 assert (x0.size() == P0.size1() && x0.size() == P0.size2());
00100 _x = x0;
00101 _P = P0;
00102 }
00103
00108 void predict(PredictFun f, ublas::matrix<T> Q, T tol=1.0e-9)
00109 {
00110 ublas::matrix<T> F = jacobian<T>(f, _x, tol);
00111 _x = f(_x);
00112 _P = ublas::prod(ublas::matrix<T>(ublas::prod(F, _P)),
00113 ublas::trans(F)) + Q;
00114 }
00115
00120 void correct(ObserveFun h, ublas::vector<T> z, ublas::matrix<T> R,
00121 T tol=1.0e-9)
00122 {
00123 ublas::matrix<T> H, S, K;
00124 ublas::vector<T> y = z - h(_x);
00125 H = jacobian<T>(h, _x, tol);
00126 S = ublas::prod(ublas::matrix<T>(ublas::prod(H, _P)), ublas::trans(H))
00127 + R;
00128 ublas::matrix<T> SInv(S.size1(), S.size2());
00129 invert<T>(S, SInv);
00130 K = ublas::prod(ublas::matrix<T>(ublas::prod(_P, ublas::trans(H))),
00131 SInv);
00132 _x = _x + ublas::prod(K, y);
00133 _P = ublas::prod((ublas::identity_matrix<double>(K.size1()) -
00134 ublas::matrix<T>(ublas::prod(K, H))), _P);
00135 }
00136
00138 ublas::vector<T> getState() const
00139 {
00140 return _x;
00141 }
00142
00143 private:
00144 ublas::vector<T> _x;
00145 ublas::matrix<T> _P;
00146 };
00147
00148
00149
00150
00151 template<class T>
00152 ublas::vector<T> rk4step(boost::function< ublas::vector<T> (T, ublas::vector<T>) > dxdtFxn, const T& dt, const T& t, ublas::vector<T> x)
00153 {
00154 const static T one_half = static_cast<T>(0.5);
00155 const static T one_sixth = static_cast<T>(1.0/6.0);
00156 ublas::vector<T> k1 = dt*dxdtFxn(t, x);
00157 ublas::vector<T> k2 = dt*dxdtFxn(t+one_half*dt, x + one_half*k1);
00158 ublas::vector<T> k3 = dt*dxdtFxn(t+one_half*dt, x + one_half*k2);
00159 ublas::vector<T> k4 = dt*dxdtFxn(t+dt, x + k3);
00160 return x + one_sixth*(k1+2.0*k2+2.0*k3+k4);
00161 }
00162
00163
00164 template<class T>
00165 ublas::vector<T> rk4(boost::function< ublas::vector<T> (T, ublas::vector<T>) > dxdtFxn,
00166 const T& t0, const T& tf, ublas::vector<T> x0, unsigned int numIter)
00167 {
00168 ublas::vector<T> x = x0;
00169 double dt = (tf-t0)/numIter;
00170 for (unsigned int k=0; k<=numIter; k++)
00171 {
00172 T t = static_cast<T>(k)*dt;
00173 x = rk4step(dxdtFxn, dt, t, x);
00174 }
00175 return x;
00176 }
00177
00178
00179 template<class T>
00180 ublas::vector<T> rkf45step(boost::function< ublas::vector<T> (T, ublas::vector<T>) > dxdtFxn, T& dt, T& t, ublas::vector<T> x, double err)
00181 {
00182 const static T a21 = static_cast<T>(1.0/4.0);
00183 const static T a31 = static_cast<T>(3.0/32.0);
00184 const static T a32 = static_cast<T>(9.0/32.0);
00185 const static T a41 = static_cast<T>(1932.0/2197.0);
00186 const static T a42 = static_cast<T>(-7200.0/2197.0);
00187 const static T a43 = static_cast<T>(7296.0/2197.0);
00188 const static T a51 = static_cast<T>(493.0/216.0);
00189 const static T a52 = static_cast<T>(-8.0);
00190 const static T a53 = static_cast<T>(3680.0/513.0);
00191 const static T a54 = static_cast<T>(-845.0/4104.0);
00192 const static T a61 = static_cast<T>(-8.0/27.0);
00193 const static T a62 = static_cast<T>(2.0);
00194 const static T a63 = static_cast<T>(-3544.0/2565.0);
00195 const static T a64 = static_cast<T>(1859.0/4104.0);
00196 const static T a65 = static_cast<T>(-11.0/40.0);
00197
00198 const static T b41 = static_cast<T>(16.0/135.0);
00199 const static T b42 = static_cast<T>(0.0);
00200 const static T b43 = static_cast<T>(6656.0/12825.0);
00201 const static T b44 = static_cast<T>(28561.0/56430.0);
00202 const static T b45 = static_cast<T>(-9.0/50.0);
00203 const static T b46 = static_cast<T>(2.0/55.0);
00204
00205 const static T b51 = static_cast<T>(25.0/216.0);
00206 const static T b52 = static_cast<T>(0.0);
00207 const static T b53 = static_cast<T>(1408.0/2565.0);
00208 const static T b54 = static_cast<T>(2197.0/4104.0);
00209 const static T b55 = static_cast<T>(-1.0/5.0);
00210 const static T b56 = static_cast<T>(0.0);
00211
00212 const static T c2 = static_cast<T>(1.0/4.0);
00213 const static T c3 = static_cast<T>(3.0/8.0);
00214 const static T c4 = static_cast<T>(12.0/13.0);
00215 const static T c5 = static_cast<T>(1.0);
00216 const static T c6 = static_cast<T>(1.0/2.0);
00217
00218
00219 ublas::vector<T> k1 = dt*dxdtFxn(t, x);
00220
00221
00222 ublas::vector<T> k2 = dt*dxdtFxn(t+c2*dt, x + a21*k1);
00223
00224
00225 ublas::vector<T> k3 = dt*dxdtFxn(t+c3*dt, x + a31*k1 + a32*k2);
00226
00227
00228 ublas::vector<T> k4 = dt*dxdtFxn(t+c4*dt, x + a41*k1 + a42*k2 + a43*k3);
00229
00230
00231 ublas::vector<T> k5 = dt*dxdtFxn(t+c5*dt, x + a51*k1 + a52*k2 + a53*k3 + a54*k4);
00232
00233
00234 ublas::vector<T> k6 = dt*dxdtFxn(t+c6*dt, x + a61*k1 + a62*k2 + a63*k3 + a64*k4 + a65*k5);
00235
00236
00237 ublas::vector<T> x4 = x + b41*k1 + b42*k2 + b43*k3 + b44*k4 + b45*k5 + b46*k6;
00238
00239
00240 ublas::vector<T> x5 = x + b51*k1 + b52*k2 + b53*k3 + b54*k4 + b55*k5 + b56*k6;
00241
00242
00243
00244 double diff = ublas::inner_prod(x5-x4, x5-x4);
00245 if (diff < err)
00246 {
00247 t += dt;
00248 x = x5;
00249 }
00250 dt = pow(err/(2.0*diff), 0.25)*dt;
00251 return x;
00252 }
00253
00254
00255 template<class T>
00256 ublas::vector<T> rkf45(boost::function< ublas::vector<T> (T, ublas::vector<T>) > dxdtFxn, T t0, T tf, ublas::vector<T> x0, double err)
00257 {
00258 ublas::vector<T> x = x0;
00259 double dt = 0.1*tf;
00260 double t = t0;
00261 while(t < tf)
00262 {
00263 dt = minimum(dt, tf-t);
00264 x = rkf45step(dxdtFxn, dt, t, x, err);
00265 }
00266 return x;
00267 }
00268
00269
00271 template <typename T>
00272 bool lmStep(boost::function< ublas::vector<T>
00273 (ublas::vector<T>) > f, ublas::vector<T>& p, ublas::vector<T> z,
00274 T& gain, T dgain=2.0, T tol=1.0e-9)
00275 {
00276
00277 ublas::matrix<T> J = jacobian<T>(f, p, tol);
00278
00279
00280 ublas::vector<T> e1v = z - f(p);
00281 T e1 = ublas::inner_prod(e1v, e1v);
00282
00283
00284 ublas::matrix<T> pInvJ(p.size(), p.size());
00285 if (!(invert<T>(ublas::prod(ublas::trans(J), J) +
00286 gain*ublas::identity_matrix<T>(p.size()), pInvJ)))
00287 return false;
00288
00289
00290 ublas::vector<T> dp = ublas::prod(ublas::prod(pInvJ, trans(J)), e1v);
00291 ublas::vector<T> e2v = z - f(p+dp);
00292 T e2 = ublas::inner_prod(e2v, e2v);
00293 if (e2 < e1)
00294 {
00295 p += dp;
00296 gain /= dgain;
00297 }
00298 else
00299 {
00300 gain *= dgain;
00301 }
00302 return true;
00303 }
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370 }
00371
00372 #endif