00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <labust/navigation/Estimator3DExtended.hpp>
00038 #include <labust/navigation/LDTravModelExtended.hpp>
00039 #include <labust/tools/GeoUtilities.hpp>
00040 #include <labust/tools/MatrixLoader.hpp>
00041 #include <labust/tools/conversions.hpp>
00042 #include <labust/tools/DynamicsLoader.hpp>
00043 #include <labust/math/NumberManipulation.hpp>
00044 #include <labust/simulation/DynamicsParams.hpp>
00045 #include <labust/navigation/KFModelLoader.hpp>
00046
00047
00048 #include <auv_msgs/NavSts.h>
00049 #include <auv_msgs/BodyForceReq.h>
00050 #include <geometry_msgs/TwistStamped.h>
00051
00052 #include <ros/ros.h>
00053
00054 #include <boost/bind.hpp>
00055
00056 using namespace labust::navigation;
00057
00058 Estimator3D::Estimator3D():
00059 tauIn(KFNav::vector::Zero(KFNav::inputSize)),
00060 measurements(KFNav::vector::Zero(KFNav::stateNum)),
00061 newMeas(KFNav::vector::Zero(KFNav::stateNum)),
00062 alt(0),
00063 useYawRate(false),
00064 dvl_model(0){this->onInit();};
00065
00066 void Estimator3D::onInit()
00067 {
00068 ros::NodeHandle nh, ph("~");
00069
00070 configureNav(nav,nh);
00071
00072 stateHat = nh.advertise<auv_msgs::NavSts>("stateHat",1);
00073 stateMeas = nh.advertise<auv_msgs::NavSts>("meas",1);
00074 currentsHat = nh.advertise<geometry_msgs::TwistStamped>("currentsHat",1);
00075
00076 tauAch = nh.subscribe<auv_msgs::BodyForceReq>("tauAch", 1, &Estimator3D::onTau,this);
00077 depth = nh.subscribe<std_msgs::Float32>("depth", 1, &Estimator3D::onDepth, this);
00078 altitude = nh.subscribe<std_msgs::Float32>("altitude", 1, &Estimator3D::onAltitude, this);
00079 modelUpdate = nh.subscribe<navcon_msgs::ModelParamsUpdate>("model_update", 1, &Estimator3D::onModelUpdate,this);
00080
00081
00082 ph.param("dvl_model",dvl_model, dvl_model);
00083 nav.useDvlModel(dvl_model);
00084 ph.param("imu_with_yaw_rate",useYawRate,useYawRate);
00085
00086
00087 gps.configure(nh);
00088 dvl.configure(nh);
00089 imu.configure(nh);
00090 }
00091
00092 void Estimator3D::configureNav(KFNav& nav, ros::NodeHandle& nh)
00093 {
00094 ROS_INFO("Configure navigation.");
00095
00096 labust::simulation::DynamicsParams params;
00097 labust::tools::loadDynamicsParams(nh, params);
00098
00099 ROS_INFO("Loaded dynamics params.");
00100
00101 this->params[X].alpha = params.m + params.Ma(0,0);
00102 this->params[X].beta = params.Dlin(0,0);
00103 this->params[X].betaa = params.Dquad(0,0);
00104
00105 this->params[Y].alpha = params.m + params.Ma(1,1);
00106 this->params[Y].beta = params.Dlin(1,1);
00107 this->params[Y].betaa = params.Dquad(1,1);
00108
00109 this->params[Z].alpha = params.m + params.Ma(2,2);
00110 this->params[Z].beta = params.Dlin(2,2);
00111 this->params[Z].betaa = params.Dquad(2,2);
00112
00113 this->params[K].alpha = params.Io(0,0) + params.Ma(3,3);
00114 this->params[K].beta = params.Dlin(3,3);
00115 this->params[K].betaa = params.Dquad(3,3);
00116
00117 this->params[M].alpha = params.Io(1,1) + params.Ma(4,4);
00118 this->params[M].beta = params.Dlin(4,4);
00119 this->params[M].betaa = params.Dquad(4,4);
00120
00121 this->params[N].alpha = params.Io(2,2) + params.Ma(5,5);
00122 this->params[N].beta = params.Dlin(5,5);
00123 this->params[N].betaa = params.Dquad(5,5);
00124
00125 nav.setParameters(this->params[X], this->params[Y],
00126 this->params[Z], this->params[K],
00127 this->params[M], this->params[N]);
00128
00129 nav.initModel();
00130 labust::navigation::kfModelLoader(nav, nh, "ekfnav");
00131 }
00132
00133 void Estimator3D::onModelUpdate(const navcon_msgs::ModelParamsUpdate::ConstPtr& update)
00134 {
00135 ROS_INFO("Updating the model parameters for %d DoF.",update->dof);
00136 params[update->dof].alpha = update->alpha;
00137 if (update->use_linear)
00138 {
00139 params[update->dof].beta = update->beta;
00140 params[update->dof].betaa = 0;
00141 }
00142 else
00143 {
00144 params[update->dof].beta = 0;
00145 params[update->dof].betaa = update->betaa;
00146 }
00147 nav.setParameters(this->params[X],this->params[Y],
00148 this->params[Z], this->params[K],
00149 this->params[M],this->params[N]);
00150 }
00151
00152 void Estimator3D::onTau(const auv_msgs::BodyForceReq::ConstPtr& tau)
00153 {
00154 tauIn(KFNav::X) = tau->wrench.force.x;
00155 tauIn(KFNav::Y) = tau->wrench.force.y;
00156 tauIn(KFNav::Z) = tau->wrench.force.z;
00157 tauIn(KFNav::Kroll) = tau->wrench.torque.x;
00158 tauIn(KFNav::M) = tau->wrench.torque.y;
00159 tauIn(KFNav::N) = tau->wrench.torque.z;
00160 };
00161
00162 void Estimator3D::onDepth(const std_msgs::Float32::ConstPtr& data)
00163 {
00164 measurements(KFNav::zp) = data->data;
00165 newMeas(KFNav::zp) = 1;
00166 };
00167
00168 void Estimator3D::onAltitude(const std_msgs::Float32::ConstPtr& data)
00169 {
00170 measurements(KFNav::altitude) = data->data;
00171 newMeas(KFNav::altitude) = 1;
00172 alt = data->data;
00173 };
00174
00175 void Estimator3D::processMeasurements()
00176 {
00177
00178 if ((newMeas(KFNav::xp) = newMeas(KFNav::yp) = gps.newArrived()))
00179 {
00180 measurements(KFNav::xp) = gps.position().first;
00181 measurements(KFNav::yp) = gps.position().second;
00182 ROS_INFO("Position measurements arrived.");
00183 }
00184
00185 if ((newMeas(KFNav::phi) = newMeas(KFNav::theta) = newMeas(KFNav::psi) = imu.newArrived()))
00186 {
00187 measurements(KFNav::phi) = imu.orientation()[ImuHandler::roll];
00188 measurements(KFNav::theta) = imu.orientation()[ImuHandler::pitch];
00189 measurements(KFNav::psi) = imu.orientation()[ImuHandler::yaw];
00190
00191 if ((newMeas(KFNav::r) = useYawRate))
00192 {
00193 measurements(KFNav::r) = imu.rate()[ImuHandler::r];
00194 }
00195 }
00196
00197
00198 if ((newMeas(KFNav::u) = newMeas(KFNav::v) = dvl.newArrived()))
00199 {
00200 double vx = dvl.body_speeds()[DvlHandler::u];
00201 double vy = dvl.body_speeds()[DvlHandler::v];
00202 double vxe = nav.getState()(KFNav::u);
00203 double vye = nav.getState()(KFNav::v);
00204
00205 double rvx(10),rvy(10);
00206
00207
00208 nav.calculateUVInovationVariance(nav.getStateCovariance(), rvx, rvy);
00209
00210 double cpsi = cos(nav.getState()(KFNav::psi));
00211 double spsi = sin(nav.getState()(KFNav::psi));
00212 double xc = nav.getState()(KFNav::xc);
00213 double yc = nav.getState()(KFNav::yc);
00214 switch (dvl_model)
00215 {
00216 case 1:
00217 vxe += xc*cpsi + yc*spsi;
00218 vye += -xc*spsi + yc*cpsi;
00219 break;
00220 default: break;
00221 }
00222
00223 if (fabs((vx - vxe)) > fabs(rvx))
00224 {
00225 ROS_INFO("Outlier rejected: meas=%f, est=%f, tolerance=%f", vx, nav.getState()(KFNav::u), fabs(rvx));
00226 newMeas(KFNav::u) = false;
00227 }
00228 measurements(KFNav::u) = vx;
00229
00230
00231 if (fabs((vy - vye)) > fabs(rvy))
00232 {
00233 ROS_INFO("Outlier rejected: meas=%f, est=%f, tolerance=%f", vy, nav.getState()(KFNav::v), fabs(rvy));
00234 newMeas(KFNav::v) = false;
00235 }
00236 measurements(KFNav::v) = vy;
00237
00238
00239 }
00240
00241
00242 auv_msgs::NavSts::Ptr meas(new auv_msgs::NavSts());
00243 meas->body_velocity.x = measurements(KFNav::u);
00244 meas->body_velocity.y = measurements(KFNav::v);
00245 meas->body_velocity.z = measurements(KFNav::w);
00246
00247 meas->position.north = measurements(KFNav::xp);
00248 meas->position.east = measurements(KFNav::yp);
00249 meas->position.depth = measurements(KFNav::zp);
00250 meas->altitude = measurements(KFNav::altitude);
00251
00252 meas->orientation.roll = measurements(KFNav::phi);
00253 meas->orientation.pitch = measurements(KFNav::theta);
00254 meas->orientation.yaw = labust::math::wrapRad(measurements(KFNav::psi));
00255 if (useYawRate) meas->orientation_rate.yaw = measurements(KFNav::r);
00256
00257 meas->origin.latitude = gps.origin().first;
00258 meas->origin.longitude = gps.origin().second;
00259 meas->global_position.latitude = gps.latlon().first;
00260 meas->global_position.longitude = gps.latlon().second;
00261
00262 meas->header.stamp = ros::Time::now();
00263 meas->header.frame_id = "local";
00264 stateMeas.publish(meas);
00265 }
00266
00267 void Estimator3D::publishState()
00268 {
00269 auv_msgs::NavSts::Ptr state(new auv_msgs::NavSts());
00270 const KFNav::vector& estimate = nav.getState();
00271 state->body_velocity.x = estimate(KFNav::u);
00272 state->body_velocity.y = estimate(KFNav::v);
00273 state->body_velocity.z = estimate(KFNav::w);
00274
00275 state->orientation_rate.roll = estimate(KFNav::p);
00276 state->orientation_rate.pitch = estimate(KFNav::q);
00277 state->orientation_rate.yaw = estimate(KFNav::r);
00278
00279 state->position.north = estimate(KFNav::xp);
00280 state->position.east = estimate(KFNav::yp);
00281 state->position.depth = estimate(KFNav::zp);
00282 state->altitude = estimate(KFNav::altitude);
00283
00284 state->orientation.roll = estimate(KFNav::phi);
00285 state->orientation.pitch = estimate(KFNav::theta);
00286 state->orientation.yaw = labust::math::wrapRad(estimate(KFNav::psi));
00287
00288 state->origin.latitude = gps.origin().first;
00289 state->origin.longitude = gps.origin().second;
00290 std::pair<double, double> diffAngle = labust::tools::meter2deg(state->position.north,
00291 state->position.east,
00292
00293 state->origin.latitude);
00294 state->global_position.latitude = state->origin.latitude + diffAngle.first;
00295 state->global_position.longitude = state->origin.longitude + diffAngle.second;
00296
00297 const KFNav::matrix& covariance = nav.getStateCovariance();
00298 state->position_variance.north = covariance(KFNav::xp, KFNav::xp);
00299 state->position_variance.east = covariance(KFNav::yp, KFNav::yp);
00300 state->position_variance.depth = covariance(KFNav::zp,KFNav::zp);
00301 state->orientation_variance.roll = covariance(KFNav::phi, KFNav::phi);
00302 state->orientation_variance.pitch = covariance(KFNav::theta, KFNav::theta);
00303 state->orientation_variance.yaw = covariance(KFNav::psi, KFNav::psi);
00304
00305 state->header.stamp = ros::Time::now();
00306 state->header.frame_id = "local";
00307 stateHat.publish(state);
00308
00309 geometry_msgs::TwistStamped::Ptr current(new geometry_msgs::TwistStamped());
00310 current->twist.linear.x = estimate(KFNav::xc);
00311 current->twist.linear.y = estimate(KFNav::yc);
00312
00313 current->header.stamp = ros::Time::now();
00314 current->header.frame_id = "local";
00315 currentsHat.publish(current);
00316 }
00317
00318 void Estimator3D::start()
00319 {
00320 ros::NodeHandle ph("~");
00321 double Ts(0.1);
00322 ph.param("Ts",Ts,Ts);
00323 ros::Rate rate(1/Ts);
00324 nav.setTs(Ts);
00325
00326 while (ros::ok())
00327 {
00328 nav.predict(tauIn);
00329 processMeasurements();
00330 bool newArrived(false);
00331 for(size_t i=0; i<newMeas.size(); ++i) if ((newArrived = newMeas(i))) break;
00332 if (newArrived) nav.correct(nav.update(measurements, newMeas));
00333
00334
00335 publishState();
00336
00337
00338 tf::StampedTransform transform;
00339 transform.setOrigin(tf::Vector3(
00340 nav.getState()(KFNav::xp),
00341 nav.getState()(KFNav::yp),
00342 nav.getState()(KFNav::zp)));
00343 Eigen::Quaternion<float> q;
00344 labust::tools::quaternionFromEulerZYX(
00345 nav.getState()(KFNav::phi),
00346 nav.getState()(KFNav::theta),
00347 nav.getState()(KFNav::psi),q);
00348 transform.setRotation(tf::Quaternion(q.x(),q.y(),q.z(),q.w()));
00349 broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "local", "base_link"));
00350
00351 rate.sleep();
00352 ros::spinOnce();
00353 }
00354 }
00355
00356 int main(int argc, char* argv[])
00357 {
00358 ros::init(argc,argv,"nav_3d");
00359 Estimator3D nav;
00360 nav.start();
00361 return 0;
00362 }
00363
00364