EKF_RTT.cpp
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  *  Author : Dula Nad
00035  *  Created: 26.03.2013.
00036  *********************************************************************/
00037 #include <labust/navigation/EKF_RTT.hpp>
00038 #include <labust/navigation/RelativeTrackingModel.hpp>
00039 
00040 #include <labust/tools/GeoUtilities.hpp>
00041 #include <labust/tools/MatrixLoader.hpp>
00042 #include <labust/tools/conversions.hpp>
00043 #include <labust/tools/DynamicsLoader.hpp>
00044 #include <labust/math/NumberManipulation.hpp>
00045 #include <labust/simulation/DynamicsParams.hpp>
00046 #include <labust/navigation/KFModelLoader.hpp>
00047 
00048 
00049 #include <auv_msgs/NavSts.h>
00050 #include <auv_msgs/BodyForceReq.h>
00051 #include <geometry_msgs/TwistStamped.h>
00052 #include <geometry_msgs/TransformStamped.h>
00053 #include <std_msgs/Float32.h>
00054 #include <std_msgs/Bool.h>
00055 #include <underwater_msgs/USBLFix.h>
00056 
00057 #include <ros/ros.h>
00058 
00059 #include <boost/bind.hpp>
00060 
00061 using namespace labust::navigation;
00062 
00063 Estimator3D::Estimator3D():
00064                 inputVec(KFNav::vector::Zero(KFNav::inputSize)),
00065                 measurements(KFNav::vector::Zero(KFNav::stateNum)),
00066                 newMeas(KFNav::vector::Zero(KFNav::measSize)),
00067                 alt(0),
00068                 xc(0),
00069                 yc(0){this->onInit();};
00070 
00071 void Estimator3D::onInit()
00072 {
00073         ros::NodeHandle nh, ph("~");
00074         //Configure the navigation
00075         configureNav(nav,nh);
00076 
00077         /* Publishers */
00078         pubStateHat = nh.advertise<auv_msgs::NavSts>("stateHatRelative",1);
00079         pubStateMeas = nh.advertise<auv_msgs::NavSts>("measRelative",1);
00080 
00081         /* Subscribers */
00082         subStateHat = nh.subscribe<auv_msgs::NavSts>("stateHat", 1, &Estimator3D::onStateHat, this);
00083         subCurrentsHat = nh.subscribe<geometry_msgs::TwistStamped>("currentsHat", 1, &Estimator3D::onCurrentsHat, this);
00084         subTargetTau = nh.subscribe<auv_msgs::BodyForceReq>("bla", 1, &Estimator3D::onTargetTau, this);
00085         subTargetDepth = nh.subscribe<std_msgs::Float32>("depth", 1,    &Estimator3D::onTargetDepth, this);
00086         subTargetHeading = nh.subscribe<std_msgs::Float32>("heading", 1, &Estimator3D::onTargetHeading, this);
00087         subUSBLfix = nh.subscribe<underwater_msgs::USBLFix>("usbl_fix", 1, &Estimator3D::onUSBLfix, this);
00088         modelUpdate = nh.subscribe<navcon_msgs::ModelParamsUpdate>("model_update", 1, &Estimator3D::onModelUpdate,this);
00089 }
00090 
00091 void Estimator3D::configureNav(KFNav& nav, ros::NodeHandle& nh){
00092 
00093         ROS_INFO("Configure navigation.");
00094 
00095         labust::simulation::DynamicsParams params;
00096         labust::tools::loadDynamicsParams(nh, params);
00097 
00098         ROS_INFO("Loaded dynamics params.");
00099 
00100         this->params[X].alpha = params.m + params.Ma(0,0);
00101         this->params[X].beta = params.Dlin(0,0);
00102         this->params[X].betaa = params.Dquad(0,0);
00103 
00104         this->params[Y].alpha = params.m + params.Ma(1,1);
00105         this->params[Y].beta = params.Dlin(1,1);
00106         this->params[Y].betaa = params.Dquad(1,1);
00107 
00108         this->params[Z].alpha = params.m + params.Ma(2,2);
00109         this->params[Z].beta = params.Dlin(2,2);
00110         this->params[Z].betaa = params.Dquad(2,2);
00111 
00112         this->params[K].alpha = params.Io(0,0) + params.Ma(3,3);
00113         this->params[K].beta = params.Dlin(3,3);
00114         this->params[K].betaa = params.Dquad(3,3);
00115 
00116         this->params[M].alpha = params.Io(1,1) + params.Ma(4,4);
00117         this->params[M].beta = params.Dlin(4,4);
00118         this->params[M].betaa = params.Dquad(4,4);
00119 
00120         this->params[N].alpha = params.Io(2,2) + params.Ma(5,5);
00121         this->params[N].beta = params.Dlin(5,5);
00122         this->params[N].betaa = params.Dquad(5,5);
00123 
00124         nav.setParameters(this->params[X], this->params[Y],
00125                         this->params[Z], this->params[K],
00126                         this->params[M], this->params[N]);
00127 
00128         nav.initModel();
00129         labust::navigation::kfModelLoader(nav, nh, "ekfnav_rtt");
00130 }
00131 
00132 void Estimator3D::onModelUpdate(const navcon_msgs::ModelParamsUpdate::ConstPtr& update)
00133 {
00134         ROS_INFO("Updating the model parameters for %d DoF.",update->dof);
00135         params[update->dof].alpha = update->alpha;
00136         if (update->use_linear)
00137         {
00138                 params[update->dof].beta = update->beta;
00139                 params[update->dof].betaa = 0;
00140         }
00141         else
00142         {
00143                 params[update->dof].beta = 0;
00144                 params[update->dof].betaa = update->betaa;
00145         }
00146         nav.setParameters(this->params[X],this->params[Y],
00147                         this->params[Z], this->params[K],
00148                         this->params[M],this->params[N]);
00149 }
00150 
00151 void Estimator3D::onStateHat(const auv_msgs::NavSts::ConstPtr& data){
00152 
00153         u = data->body_velocity.x;
00154         v = data->body_velocity.y;
00155 
00156         inputVec(KFNav::psi) = data->orientation.yaw;
00157 }
00158 
00159 void Estimator3D::onCurrentsHat(const  geometry_msgs::TwistStamped::ConstPtr& data){
00160 
00161         xc = data->twist.linear.x;
00162         yc = data->twist.linear.y;
00163 }
00164 
00165 void Estimator3D::onTargetTau(const auv_msgs::BodyForceReq::ConstPtr& tau)
00166 {
00167         inputVec(KFNav::X) = tau->wrench.force.x;
00168         inputVec(KFNav::Z) = tau->wrench.force.z;
00169         inputVec(KFNav::N) = tau->wrench.torque.z;
00170 }
00171 
00172 void Estimator3D::onTargetDepth(const std_msgs::Float32::ConstPtr& data)
00173 {
00174         measurements(KFNav::depth) = data->data;
00175         newMeas(KFNav::depth) = 1;
00176 }
00177 
00178 void Estimator3D::onTargetHeading(const std_msgs::Float32::ConstPtr& data){
00179 
00180         measurements(KFNav::psi_t) = data->data;
00181         newMeas(KFNav::psi_t) = 1;
00182 }
00183 
00184 void Estimator3D::onUSBLfix(const underwater_msgs::USBLFix::ConstPtr& data){
00185 
00186 
00187 
00188         measurements(KFNav::d) = data->range;
00189         newMeas(KFNav::d) = 1;
00190 
00191         measurements(KFNav::theta) = data->bearing;
00192         newMeas(KFNav::theta) = 1;
00193 
00194         measurements(KFNav::delta_xm) = data->relative_position.x;
00195         newMeas(KFNav::delta_xm) = 1;
00196 
00197         measurements(KFNav::delta_ym) = data->relative_position.y;
00198         newMeas(KFNav::delta_ym) = 1;
00199 
00200         measurements(KFNav::delta_zm) = data->relative_position.z;
00201         newMeas(KFNav::delta_zm) = 1;
00202 }
00203 
00204 
00205 void Estimator3D::processMeasurements()
00206 {
00207 
00208         //Publish measurements
00209         //auv_msgs::NavSts::Ptr meas(new auv_msgs::NavSts());
00210         //meas->body_velocity.x = measurements(KFNav::u);
00211         //meas->body_velocity.y = measurements(KFNav::v);
00212         //meas->body_velocity.z = measurements(KFNav::w);
00213 
00214         //meas->position.north = measurements(KFNav::xp);
00215         //meas->position.east = measurements(KFNav::yp);
00216         //meas->position.depth = measurements(KFNav::zm);
00217         //meas->altitude = measurements(KFNav::altitude);
00218 
00219         //meas->orientation.roll = measurements(KFNav::phi);
00220         //meas->orientation.pitch = measurements(KFNav::theta);
00221         //meas->orientation.yaw = labust::math::wrapRad(measurements(KFNav::psim));
00222         //if (useYawRate)       meas->orientation_rate.yaw = measurements(KFNav::r);
00223 
00224         //meas->origin.latitude = gps.origin().first;
00225         //meas->origin.longitude = gps.origin().second;
00226         //meas->global_position.latitude = gps.latlon().first;
00227         //meas->global_position.longitude = gps.latlon().second;
00228 
00229         //meas->header.stamp = ros::Time::now();
00230         //meas->header.frame_id = "local";
00231         //stateMeas.publish(meas);
00232 }
00233 
00234 void Estimator3D::publishState(){
00235 
00236         auv_msgs::NavSts::Ptr state(new auv_msgs::NavSts());
00237         const KFNav::vector& estimate = nav.getState();
00238 
00239         state->body_velocity.x = estimate(KFNav::u_t);
00240         state->body_velocity.z = estimate(KFNav::w_t);
00241 
00242         state->orientation_rate.yaw = estimate(KFNav::r_t);
00243 
00244         state->position.north = estimate(KFNav::delta_x);
00245         state->position.east = estimate(KFNav::delta_y);
00246         state->position.depth = estimate(KFNav::delta_z);
00247 
00248         state->orientation.yaw = labust::math::wrapRad(estimate(KFNav::psi_t));
00249 
00250         const KFNav::matrix& covariance = nav.getStateCovariance();
00251         state->position_variance.north = covariance(KFNav::delta_x, KFNav::delta_x);
00252         state->position_variance.east = covariance(KFNav::delta_y, KFNav::delta_y);
00253         state->position_variance.depth = covariance(KFNav::delta_z, KFNav::delta_z);
00254 
00255         state->orientation_variance.yaw =  covariance(KFNav::psi_t, KFNav::psi_t);
00256 
00257         state->header.stamp = ros::Time::now();
00258         state->header.frame_id = "local";
00259         pubStateHat.publish(state);
00260 }
00261 
00262 void Estimator3D::start(){
00263 
00264         ros::NodeHandle ph("~");
00265         double Ts(0.1);
00266         ph.param("Ts",Ts,Ts);
00267         ros::Rate rate(1/Ts);
00268         nav.setTs(Ts);
00269 
00270         while (ros::ok()){
00271 
00272                 inputVec(KFNav::x_dot) = xc + u*cos(inputVec(KFNav::psi)) - v*sin(inputVec(KFNav::psi));
00273                 inputVec(KFNav::y_dot) = yc + u*sin(inputVec(KFNav::psi)) + v*cos(inputVec(KFNav::psi));
00274 
00275                 nav.predict(inputVec);
00276                 processMeasurements();
00277                 bool newArrived(false);
00278                 for(size_t i=0; i<newMeas.size(); ++i)  if ((newArrived = newMeas(i))) break;
00279                 if (newArrived) nav.correct(nav.update(measurements, newMeas));
00280 
00281                 publishState();
00282 
00283                 //Send the base-link transform
00284                 geometry_msgs::TransformStamped transform;
00285                 transform.transform.translation.x = nav.getState()(KFNav::delta_x);
00286                 transform.transform.translation.y = nav.getState()(KFNav::delta_y);
00287                 transform.transform.translation.z = nav.getState()(KFNav::delta_z);
00288 
00289                 labust::tools::quaternionFromEulerZYX(0.0,
00290                                                 0.0,
00291                                                 nav.getState()(KFNav::psi),
00292                                                 transform.transform.rotation);
00293 
00294                 transform.child_frame_id = "base_link_relative";
00295                 transform.header.frame_id = "local";
00296                 transform.header.stamp = ros::Time::now();
00297                 broadcaster.sendTransform(transform);
00298 
00299                 rate.sleep();
00300                 ros::spinOnce();
00301         }
00302 }
00303 
00304 int main(int argc, char* argv[]){
00305 
00306         ros::init(argc,argv,"nav_sb");
00307         Estimator3D nav;
00308         nav.start();
00309         return 0;
00310 }
00311 
00312 


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