covpredict.h

00001 /*
00002  * Orca-Robotics Project: Components for robotics 
00003  *               http://orca-robotics.sf.net/
00004  * Copyright (c) 2004-2009 Alex Brooks, Alexei Makarenko, Tobias Kaupp
00005  *
00006  * This distribution is licensed to you under the terms described in
00007  * the LICENSE file included in this distribution.
00008  *
00009  */
00010 #ifndef ORCANAV_COV_PREDICT_H
00011 #define ORCANAV_COV_PREDICT_H
00012 
00013 namespace hydronavutil {
00014 
00015 //
00016 // The prediction step of an EKF for the covariance: 
00017 //   P = FPF' + GQG' + sigma
00018 //
00019 // The action is defined in the vehicle's local coordinate system.
00020 //
00021 // [ref: Hugh's notes, esp. slides 146 & 248]
00022 //
00023 void
00024 covPredict( double  poseX,
00025             double  poseY,
00026             double  poseT,
00027             double &Pxx,
00028             double &Pxy,
00029             double &Pxt,
00030             double &Pyy,
00031             double &Pyt,
00032             double &Ptt,
00033             double  actX,
00034             double  actY,
00035             double  actT,
00036             double  velScaleCov,
00037             double  yawScaleCov,
00038             double  scaledStabNoisePxx,
00039             double  scaledStabNoisePyy,
00040             double  scaledStabNoisePtt );
00041 
00042 //
00043 // The prediction step of an EKF for the covariance, assuming known starting point:
00044 //   P = GQG' + sigma
00045 //
00046 // The action is defined in the vehicle's local coordinate system.
00047 //
00048 // [ref: Hugh's notes, esp. slides 146 & 248]
00049 //
00050 void
00051 covPredictFromCertainStart( double  poseX,
00052                             double  poseY,
00053                             double  poseT,
00054                             double &Pxx,
00055                             double &Pxy,
00056                             double &Pxt,
00057                             double &Pyy,
00058                             double &Pyt,
00059                             double &Ptt,
00060                             double  actX,
00061                             double  actY,
00062                             double  actT,
00063                             double  velScaleCov,
00064                             double  yawScaleCov,
00065                             double  scaledStabNoisePxx,
00066                             double  scaledStabNoisePyy,
00067                             double  scaledStabNoisePtt );
00068 
00069 
00070 // for debugging
00071 void
00072 printP( double Pxx,
00073         double Pxy,
00074         double Pyy,
00075         double Pxt,
00076         double Pyt,
00077         double Ptt );
00078 
00079 }
00080 
00081 #endif