EKF_3D_USBLModel.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *  EKF3D_USBLModel.cpp
00003  *
00004  *  Created on: Mar 26, 2013
00005  *      Author: Dula Nad
00006  *
00007  *   Modified on: Feb 27, 2015
00008  *      Author: Filip Mandic
00009  *
00010  ********************************************************************/
00011 
00012 /*********************************************************************
00013 * Software License Agreement (BSD License)
00014 *
00015 *  Copyright (c) 2010, LABUST, UNIZG-FER
00016 *  All rights reserved.
00017 *
00018 *  Redistribution and use in source and binary forms, with or without
00019 *  modification, are permitted provided that the following conditions
00020 *  are met:
00021 *
00022 *   * Redistributions of source code must retain the above copyright
00023 *     notice, this list of conditions and the following disclaimer.
00024 *   * Redistributions in binary form must reproduce the above
00025 *     copyright notice, this list of conditions and the following
00026 *     disclaimer in the documentation and/or other materials provided
00027 *     with the distribution.
00028 *   * Neither the name of the LABUST nor the names of its
00029 *     contributors may be used to endorse or promote products derived
00030 *     from this software without specific prior written permission.
00031 *
00032 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00033 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00034 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00035 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00036 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00037 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00038 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00039 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00040 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00041 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00042 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00043 *  POSSIBILITY OF SUCH DAMAGE.
00044 *********************************************************************/
00045 #include <labust/navigation/EKF_3D_USBLModel.hpp>
00046 
00047 using namespace labust::navigation;
00048 
00049 #include <vector>
00050 
00051 #include <ros/ros.h>
00052 
00053 EKF_3D_USBLModel::EKF_3D_USBLModel():
00054                 dvlModel(0),
00055                 xdot(0),
00056                 ydot(0){
00057 
00058         this->initModel();
00059 };
00060 
00061 EKF_3D_USBLModel::~EKF_3D_USBLModel(){};
00062 
00063 void EKF_3D_USBLModel::initModel(){
00064 
00065   x = vector::Zero(stateNum);
00066   xdot = 0;
00067   ydot = 0;
00068   /*** Setup the transition matrix ***/
00069   derivativeAW();
00070   R0 = R;
00071   V0 = V;
00072   //std::cout<<"R:"<<R<<"\n"<<V<<std::endl;
00073 }
00074 
00075 void EKF_3D_USBLModel::calculateXYInovationVariance(const EKF_3D_USBLModel::matrix& P, double& xin,double &yin){
00076 
00077         xin = sqrt(P(xp,xp)) + sqrt(R0(xp,xp));
00078         yin = sqrt(P(yp,yp)) + sqrt(R0(yp,yp));
00079 }
00080 
00081 double EKF_3D_USBLModel::calculateAltInovationVariance(const EKF_3D_USBLModel::matrix& P){
00082 
00083         return sqrt(P(altitude,altitude)) + sqrt(R0(altitude,altitude));
00084 }
00085 
00086 void EKF_3D_USBLModel::calculateUVInovationVariance(const EKF_3D_USBLModel::matrix& P, double& uin,double &vin){
00087 
00088         uin = sqrt(P(u,u)) + sqrt(R0(v,v));
00089         vin = sqrt(P(v,v)) + sqrt(R0(v,v));
00090 }
00091 
00092 void EKF_3D_USBLModel::step(const input_type& input){
00093 
00094   x(u) += Ts*(-surge.Beta(x(u))/surge.alpha*x(u) + 1/surge.alpha * input(X));
00095   x(v) += Ts*(-sway.Beta(x(v))/sway.alpha*x(v) + 1/sway.alpha * input(Y));
00096   x(w) += Ts*(-heave.Beta(x(w))/heave.alpha*x(w) + 1/heave.alpha * (input(Z) + x(buoyancy)));
00097   //x(p) += Ts*(-roll.Beta(x(p))/roll.alpha*x(p) + 1/roll.alpha * (input(Kroll) + x(roll_restore)));
00098   //x(q) += Ts*(-pitch.Beta(x(p))/pitch.alpha*x(q) + 1/pitch.alpha * (input(M) + x(pitch_restore)));
00099   x(r) += Ts*(-yaw.Beta(x(r))/yaw.alpha*x(r) + 1/yaw.alpha * input(N) + x(b));
00100 
00101   xdot = x(u)*cos(x(psi)) - x(v)*sin(x(psi)) + x(xc);
00102   ydot = x(u)*sin(x(psi)) + x(v)*cos(x(psi)) + x(yc);
00103   x(xp) += Ts * xdot;
00104   x(yp) += Ts * ydot;
00105   x(zp) += Ts * x(w);
00106   x(altitude) += -Ts * x(w);
00107   //\todo This is not actually true since angles depend on each other
00108   //\todo Also x,y are dependent on the whole rotation matrix.
00109   //\todo We make a simplification here for testing with small angles ~10°
00110   //x(phi) += Ts * x(p);
00111   //x(theta) += Ts * x(q);
00112   x(psi) += Ts * x(r);
00113 
00114   xk_1 = x;
00115 
00116   derivativeAW();
00117 }
00118 
00119 void EKF_3D_USBLModel::derivativeAW(){
00120 
00121         A = matrix::Identity(stateNum, stateNum);
00122 
00123         A(u,u) = 1-Ts*(surge.beta + 2*surge.betaa*fabs(x(u)))/surge.alpha;
00124         A(v,v) = 1-Ts*(sway.beta + 2*sway.betaa*fabs(x(v)))/sway.alpha;
00125         A(w,w) = 1-Ts*(heave.beta + 2*heave.betaa*fabs(x(w)))/heave.alpha;
00126         A(w,buoyancy) = Ts/heave.alpha;
00127         //A(p,p) = 1-Ts*(roll.beta + 2*roll.betaa*fabs(x(p)))/roll.alpha;
00128         //A(p,roll_restore) = Ts/roll.alpha;
00129         //A(q,q) = 1-Ts*(pitch.beta + 2*pitch.betaa*fabs(x(q)))/pitch.alpha;
00130         //A(q,pitch_restore) = Ts/pitch.alpha;
00131         A(r,r) = 1-Ts*(yaw.beta + 2*yaw.betaa*fabs(x(r)))/yaw.alpha;
00132         A(r,b) = Ts;
00133 
00134         A(xp,u) = Ts*cos(x(psi));
00135         A(xp,v) = -Ts*sin(x(psi));
00136         A(xp,psi) = Ts*(-x(u)*sin(x(psi)) - x(v)*cos(x(psi)));
00137         A(xp,xc) = Ts;
00138 
00139         A(yp,u) = Ts*sin(x(psi));
00140         A(yp,v) = Ts*cos(x(psi));
00141         A(yp,psi) = Ts*(x(u)*cos(x(psi)) - x(v)*sin(x(psi)));
00142         A(yp,yc) = Ts;
00143 
00144         A(zp,w) = Ts;
00145         //\todo If you don't want the altitude to contribute comment this out.
00146         A(altitude,w) = -Ts;
00147 
00148         //A(phi,p) = Ts;
00149         //A(theta,q) = Ts;
00150         A(psi,r) = Ts;
00151 }
00152 
00153 const EKF_3D_USBLModel::output_type& EKF_3D_USBLModel::update(vector& measurements, vector& newMeas){
00154 
00155         std::vector<size_t> arrived;
00156         std::vector<double> dataVec;
00157 
00158         for (size_t i=0; i<newMeas.size(); ++i)
00159         {
00160                 if (newMeas(i))
00161                 {
00162                         arrived.push_back(i);
00163                         dataVec.push_back(measurements(i));
00164                         newMeas(i) = 0;
00165                 }
00166         }
00167 
00168         if (dvlModel != 0) derivativeH();
00169 
00170         measurement.resize(arrived.size());
00171         H = matrix::Zero(arrived.size(),stateNum);
00172         y = vector::Zero(arrived.size());
00173         R = matrix::Zero(arrived.size(),arrived.size());
00174         V = matrix::Zero(arrived.size(),arrived.size());
00175 
00176         for (size_t i=0; i<arrived.size();++i)
00177         {
00178                 measurement(i) = dataVec[i];
00179 
00180                 if (dvlModel != 0)
00181                 {
00182                         H.row(i)=Hnl.row(arrived[i]);
00183                         y(i) = ynl(arrived[i]);
00184                 }
00185                 else
00186                 {
00187                         H(i,arrived[i]) = 1;
00188                         y(i) = x(arrived[i]);
00189                 }
00190 
00191                 for (size_t j=0; j<arrived.size(); ++j)
00192                 {
00193                         R(i,j)=R0(arrived[i],arrived[j]);
00194                         V(i,j)=V0(arrived[i],arrived[j]);
00195                 }
00196         }
00197 
00198         /*** Debug Print
00199         std::cout<<"Setup H:"<<H<<std::endl;
00200         std::cout<<"Setup R:"<<R<<std::endl;
00201         std::cout<<"Setup V:"<<V<<std::endl;
00202         ***/
00203         return measurement;
00204 }
00205 
00206 void EKF_3D_USBLModel::estimate_y(output_type& y){
00207         y=this->y;
00208 }
00209 
00210 void EKF_3D_USBLModel::derivativeH(){
00211 
00212         Hnl = matrix::Zero(measSize,stateNum); // Prije je bilo identity
00213         Hnl.topLeftCorner(stateNum,stateNum) = matrix::Identity(stateNum,stateNum);
00214 
00215         ynl = vector::Zero(measSize);
00216         ynl.head(stateNum) = matrix::Identity(stateNum,stateNum)*x;
00217 
00218         switch (dvlModel){
00219         case 1:
00220                 /*** Correct the nonlinear part ***/
00221                 ynl(u) = x(u)+x(xc)*cos(x(psi))+x(yc)*sin(x(psi));
00222                 ynl(v) = x(v)-x(xc)*sin(x(psi))+x(yc)*cos(x(psi));
00223 
00224                 //Correct for the nonlinear parts
00225                 Hnl(u,u) = 1;
00226                 Hnl(u,xc) = cos(x(psi));
00227                 Hnl(u,yc) = sin(x(psi));
00228                 Hnl(u,psi) = -x(xc)*sin(x(psi)) + x(yc)*cos(x(psi));
00229 
00230                 Hnl(v,v) = 1;
00231                 Hnl(v,xc) = -sin(x(psi));
00232                 Hnl(v,yc) = cos(x(psi));
00233                 Hnl(v,psi) = -x(xc)*cos(x(psi)) - x(yc)*sin(x(psi));
00234                 break;
00235 
00236         case 2:
00237                 /*** Correct the nonlinear part ***/
00238                 y(u) = x(u)*cos(x(psi)) - x(v)*sin(x(psi)) + x(xc);
00239                 y(v) = x(u)*sin(x(psi)) + x(v)*cos(x(psi)) + x(yc);
00240 
00241             /*** Correct for the nonlinear parts ***/
00242                 Hnl(u,xc) = 1;
00243                 Hnl(u,u) = cos(x(psi));
00244                 Hnl(u,v) = -sin(x(psi));
00245                 Hnl(u,psi) = -x(u)*sin(x(psi)) - x(v)*cos(x(psi));
00246 
00247                 Hnl(v,yc) = 1;
00248                 Hnl(v,u) = sin(x(psi));
00249                 Hnl(v,v) = cos(x(psi));
00250                 Hnl(v,psi) = x(u)*cos(x(psi)) - x(v)*sin(x(psi));
00251                 break;
00252         }
00253 
00254 //      double rng  = sqrt(pow((x(xp)-x(xb)),2)+pow((x(yp)-x(yb)),2)+pow((x(zp)-x(zb)),2));
00255 //
00256 //      //if(rng == 0) rng = 0.1;
00257 //
00258 //      if(rng<0.0001){
00259 //              rng = 0.0001;
00260 //      }
00261 //
00262 //      double delta_xbp = x(xb)-x(xp);
00263 //      //ROS_ERROR("delta: %f",delta_xbp);
00264 //      if(abs(delta_xbp)<0.000000001){
00265 //              delta_xbp = (delta_xbp<0)?-0.000000001:0.000000001;
00266 //      }
00267 //
00268 //      //ROS_ERROR("delta after: %f",delta_xbp);
00269 //
00270 //      ynl(range) = rng;
00271 //      ynl(bearing) = atan2((x(yp)-x(yb)),(x(xp)-x(xb)))-0*x(psi);
00272 //      ynl(elevation) = asin((x(zp)-x(zb))/rng);
00273 //
00274 //      Hnl(range, xp)  = (x(xp)-x(xb))/rng;
00275 //      Hnl(range, yp)  = (x(yp)-x(yb))/rng;
00276 //      Hnl(range, zp)  = (x(zp)-x(zb))/rng;
00277 //
00278 //      Hnl(range, xb)  = -(x(xp)-x(xb))/rng;
00279 //      Hnl(range, yb)  = -(x(yp)-x(yb))/rng;
00280 //      Hnl(range, zb)  = -(x(zp)-x(zb))/rng;
00281 //
00282 //      Hnl(bearing, xp) = (x(yb)-x(yp))/(pow(delta_xbp,2)*(pow((x(yb)-x(yp))/delta_xbp,2) + 1));
00283 //      Hnl(bearing, yp) = -1/((delta_xbp)*(pow((x(yb) - x(yp))/delta_xbp,2) + 1));
00284 //      Hnl(bearing, xb) = -(x(yb) - x(yp))/(pow(delta_xbp,2)*(pow((x(yb) - x(yp))/delta_xbp,2) + 1));
00285 //      Hnl(bearing, yb) = 1/((delta_xbp)*(pow((x(yb) - x(yp))/delta_xbp,2) + 1));
00286 //      //Hnl(bearing, psi) = -1;
00287 
00288 
00289         double rng  = sqrt(pow((x(xp)-x(xb)),2)+pow((x(yp)-x(yb)),2)+pow((x(zp)-x(zb)),2));
00290         double delta_x = (x(xb)-x(xp));
00291         double delta_y = (x(yb)-x(yp));
00292 
00293         if(rng<0.00001)
00294                 rng = 0.00001;
00295 
00296         if(abs(delta_x)<0.00001)
00297                 delta_x = (delta_x<0)?-0.00001:0.00001;
00298 
00299         if(abs(delta_y)<0.00001)
00300                 delta_y = (delta_y<0)?-0.00001:0.00001;
00301 
00302         ynl(range) = rng;
00303         ynl(bearing) = atan2(delta_y,delta_x) -1*x(psi);
00304         ynl(elevation) = asin((x(zp)-x(zb))/rng);
00305 
00306         Hnl(range, xp)  = -(x(xb)-x(xp))/rng;
00307         Hnl(range, yp)  = -(x(yb)-x(yp))/rng;
00308         Hnl(range, zp)  = -(x(zb)-x(zp))/rng;
00309 
00310         Hnl(range, xb)  = (x(xb)-x(xp))/rng;
00311         Hnl(range, yb)  = (x(yb)-x(yp))/rng;
00312         Hnl(range, zb)  = (x(zb)-x(zp))/rng;
00313 
00314         Hnl(bearing, xp) = delta_y/(delta_x*delta_x+delta_y*delta_y);
00315         Hnl(bearing, yp) = -delta_x/(delta_x*delta_x+delta_y*delta_y);
00316         Hnl(bearing, xb) = -delta_y/(delta_x*delta_x+delta_y*delta_y);
00317         Hnl(bearing, yb) = delta_x/(delta_x*delta_x+delta_y*delta_y);
00318 
00319         Hnl(bearing, psi) = -1;
00320 
00321 
00322 
00323         // Nadi gresku u elevationu i sredi singularitete
00324 //      double part1 = (x(zb) - x(zp))/(sqrt(1 - pow((x(zb) - x(zp)),2)/pow(rng,2))*(pow((rng),3)));
00325 //      double part2 = (x(xb)*x(xb) - 2*x(xb)*x(xp) + x(xp)*x(xp) + x(yb)*x(yb) - 2*x(yb)*x(yp) + x(yp)*x(yp))/(sqrt(1 - pow((x(zb) - x(zp)),2)/pow(rng,2))*(pow((rng),3)));
00326 //
00327 //      Hnl(elevation,xp) = -((x(xb) - x(xp))*part1);
00328 //      Hnl(elevation,yp) = -((x(yb) - x(yp))*part1);
00329 //      Hnl(elevation,zp) = part2;
00330 //      Hnl(elevation,xb) = ((x(xb) - x(xp))*part1);
00331 //      Hnl(elevation,yb) = ((x(yb) - x(yp))*part1);
00332 //      Hnl(elevation,zb) = -part2;
00333 }
00334 


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