RelativeTrackingModel.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 *  Modified on: Jun 04, 2014
00038 *  Modified by: Filip Mandic
00039 *
00040 *********************************************************************/
00041 #ifndef RELATIVETRACKINGMODEL_HPP_
00042 #define RELATIVETRACKINGMODEL_HPP_
00043 
00044 //#include <>
00045 #include <labust/navigation/SSModel.hpp>
00046 
00047 namespace labust{
00048   namespace navigation{
00052     class RelativeTrackingModel : public SSModel<double>{
00053 
00054         typedef SSModel<double> Base;
00055     public:
00056       typedef vector input_type;
00057       typedef vector output_type;
00058 
00059       struct ModelParams{
00060           ModelParams():
00061                   alpha(1),
00062                   beta(1),
00063                   betaa(0){};
00064 
00065           ModelParams(double alpha, double beta, double betaa):
00066                   alpha(alpha),
00067                   beta(beta),
00068                   betaa(betaa){}
00069 
00070           inline double Beta(double val){
00071                   return beta + betaa*fabs(val);
00072           }
00073 
00074           double alpha, beta, betaa;
00075       };
00076 
00077       enum {delta_x = 0,delta_y, delta_z, psi_t, u_t, w_t, r_t, stateNum}; /* State vector - Full */
00078       enum {d = 0, theta, depth, psi_tm, delta_xm, delta_ym, delta_zm, measSize}; /* Measurement vector */
00079       enum {x_dot = 0, y_dot, psi, X, Z, N, inputSize}; /* Input vector */
00080 
00084       RelativeTrackingModel();
00088       ~RelativeTrackingModel();
00089 
00095       void step(const input_type& input);
00101       void estimate_y(output_type& y);
00105       void initModel();
00106 
00110       const output_type& update(vector& measurements, vector& newMeas);
00111 
00115       void setParameters(const ModelParams& surge,
00116                   const ModelParams& sway,
00117                   const ModelParams& heave,
00118                   const ModelParams& roll,
00119                   const ModelParams& pitch,
00120                   const ModelParams& yaw)
00121       {
00122           this->surge = surge;
00123           this->sway = sway;
00124           this->heave = heave;
00125           this->roll = roll;
00126           this->pitch = pitch;
00127           this->yaw = yaw;
00128       }
00129 
00130       void calculateXYInovationVariance(const matrix& P, double& xin,double &yin);
00131       void calculateUVInovationVariance(const matrix& P, double& uin,double &vin);
00132       double calculateAltInovationVariance(const matrix& P);
00133 
00137       inline void getNEDSpeed(double& xdot, double& ydot)
00138       {
00139         xdot = this->xdot;
00140         ydot = this->ydot;
00141       }
00142 
00143       inline void useDvlModel(int flag){this->dvlModel = flag;};
00144 
00145     protected:
00149       void derivativeAW();
00153        void derivativeHV(int num);
00157        void derivativeH();
00161       ModelParams surge,sway,heave,roll,pitch,yaw;
00165       output_type measurement;
00169       double xdot,ydot;
00173       int dvlModel;
00177       matrix Hnl;
00181       vector ynl,y;
00182     };
00183   }
00184 }
00185 
00186 /* SBMODEL_HPP_ */
00187 #endif


labust_navigation
Author(s): Gyula Nagy
autogenerated on Fri Aug 28 2015 11:23:33