XYModel.hpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, LABUST, UNIZG-FER
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the LABUST nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *
00034 *  Created on: Feb 25, 2013
00035 *  Author: Dula Nad
00036 *********************************************************************/
00037 #ifndef XYMODEL_HPP_
00038 #define XYMODEL_HPP_
00039 #include <labust/navigation/SSModel.hpp>
00040 
00041 namespace labust
00042 {
00043   namespace navigation
00044   {
00048     class XYModel : public SSModel<double>
00049     {
00050         typedef SSModel<double> Base;
00051     public:
00052       typedef vector input_type;
00053       typedef vector output_type;
00054 
00055       struct ModelParams
00056       {
00057           ModelParams():
00058                   alpha(1),
00059                   beta(1),
00060                   betaa(0){};
00061 
00062           ModelParams(double alpha, double beta, double betaa):
00063                   alpha(alpha),
00064                   beta(beta),
00065                   betaa(betaa){}
00066 
00067           inline double Beta(double val)
00068           {
00069                   return beta + betaa*fabs(val);
00070           }
00071 
00072           double alpha, beta, betaa;
00073       };
00074 
00075       enum {u=0,v,r,xp,yp,psi,xc,yc,b1,b2};
00076       enum {stateNum = 10};
00077       enum {inputSize = 3};
00078       enum {measSize = 3};
00079       enum {X=0,Y,N};
00080       enum {x_m=0,y_m,psi_m,psiOnly_m = 0};
00081         
00085       XYModel();
00089       ~XYModel();
00090 
00096       void step(const input_type& input);
00102       void estimate_y(output_type& y);
00106       void initModel();
00107 
00111       const output_type& update(vector& measurements, vector& newMeas);
00112 
00116       const output_type& yawUpdate(double yaw);
00120       const output_type& fullUpdate(double x,
00121                   double y,
00122                   double yaw);
00126       const output_type& positionUpdate(double x,
00127                   double y);
00131       void setParameters(const ModelParams& surge,
00132                   const ModelParams& sway,
00133                   const ModelParams& yaw)
00134       {
00135           this->surge = surge;
00136           this->sway = sway;
00137           this->yaw = yaw;
00138       }
00139 
00140       void calculateXYInovationVariance(const matrix& P, double& xin,double &yin);
00141 
00145       inline void getNEDSpeed(double& xdot, double& ydot)
00146       {
00147         xdot = this->xdot;
00148         ydot = this->ydot;
00149       }
00150 
00151 
00152     protected:
00156       void derivativeAW();
00160        void derivativeHV(int num);
00164       ModelParams surge,sway,yaw;
00168       output_type measurement;
00172       double xdot,ydot;
00173     };
00174   }
00175 }
00176 
00177 /* XYMODEL_HPP_ */
00178 #endif


labust_navigation
Author(s): Gyula Nagy
autogenerated on Fri Feb 7 2014 11:36:19