#include <EKF3D.hpp>
Classes | |
struct | FilterState |
Public Member Functions | |
Estimator3D () | |
Estimator3D () | |
Estimator3D () | |
void | onInit () |
void | onInit () |
void | onInit () |
void | start () |
void | start () |
void | start () |
Private Types | |
enum | { X = 0, Y, Z, K, M, N, DoF } |
enum | { X = 0, Y, Z, K, M, N, DoF } |
enum | { X = 0, Y, Z, K, M, N, DoF } |
typedef labust::navigation::KFCore < labust::navigation::LDTravModel > | KFNav |
typedef labust::navigation::KFCore < labust::navigation::RelativeTrackingModel > | KFNav |
typedef labust::navigation::KFCore < labust::navigation::EKF_3D_USBLModel > | KFNav |
Private Member Functions | |
int | calculateDelaySteps (double measTime, double arrivalTime) |
void | configureNav (KFNav &nav, ros::NodeHandle &nh) |
void | configureNav (KFNav &nav, ros::NodeHandle &nh) |
void | configureNav (KFNav &nav, ros::NodeHandle &nh) |
void | deltaPosCallback (const auv_msgs::NED::ConstPtr &msg) |
void | deltaPosCallback (const auv_msgs::NED::ConstPtr &msg) |
void | KFmodeCallback (const std_msgs::Bool::ConstPtr &msg) |
void | KFmodeCallback (const std_msgs::Bool::ConstPtr &msg) |
void | onAltitude (const std_msgs::Float32::ConstPtr &data) |
void | onAltitude (const std_msgs::Float32::ConstPtr &data) |
void | onCurrentsHat (const geometry_msgs::TwistStamped::ConstPtr &data) |
void | onDepth (const std_msgs::Float32::ConstPtr &data) |
void | onDepth (const std_msgs::Float32::ConstPtr &data) |
void | onModelUpdate (const navcon_msgs::ModelParamsUpdate::ConstPtr &update) |
void | onModelUpdate (const navcon_msgs::ModelParamsUpdate::ConstPtr &update) |
void | onModelUpdate (const navcon_msgs::ModelParamsUpdate::ConstPtr &update) |
void | onReset (const std_msgs::Bool::ConstPtr &reset) |
void | onReset (const std_msgs::Bool::ConstPtr &reset) |
void | onStateHat (const auv_msgs::NavSts::ConstPtr &data) |
void | onTargetDepth (const std_msgs::Float32::ConstPtr &data) |
void | onTargetHeading (const std_msgs::Float32::ConstPtr &data) |
void | onTargetTau (const auv_msgs::BodyForceReq::ConstPtr &tau) |
void | onTau (const auv_msgs::BodyForceReq::ConstPtr &tau) |
void | onTau (const auv_msgs::BodyForceReq::ConstPtr &tau) |
void | onUSBLfix (const underwater_msgs::USBLFix::ConstPtr &data) |
void | onUSBLfix (const underwater_msgs::USBLFix::ConstPtr &data) |
void | onUseGyro (const std_msgs::Bool::ConstPtr &use_gyro) |
void | onUseGyro (const std_msgs::Bool::ConstPtr &use_gyro) |
void | processMeasurements () |
void | processMeasurements () |
void | processMeasurements () |
void | publishState () |
void | publishState () |
void | publishState () |
Private Attributes | |
bool | absoluteEKF |
double | alt |
ros::Subscriber | altitude |
tf2_ros::TransformBroadcaster | broadcaster |
ros::Publisher | buoyancyHat |
double | compassVariance |
ros::Publisher | currentsHat |
double | currentTime |
double | delay_time |
double | delayTime |
float | deltaXpos |
float | deltaYpos |
ros::Subscriber | depth |
DvlHandler | dvl |
int | dvl_model |
ros::Time | dvl_time |
Last DVL measurement received time. | |
double | dvl_timeout |
Timeout value for the DVL. | |
bool | enableBearing |
bool | enableDelay |
bool | enableElevation |
bool | enableRange |
GPSHandler | gps |
double | gyroVariance |
ImuHandler | imu |
KFNav::vector | inputVec |
bool | KFmode |
bool | KFmodePast |
double | max_dvl |
Maximum dvl speed for sanity checks. | |
boost::mutex | meas_mux |
Mutex for data protection. | |
KFNav::vector | measDelay |
KFNav::vector | measurements |
double | min_altitude |
Minimum safe altitude that is accepted as measurement. | |
ros::Subscriber | modelUpdate |
KFNav | nav |
KFNav::vector | newMeas |
labust::tools::OutlierRejection | OR |
KFNav::ModelParams | params [DoF] |
std::deque< FilterState > | pastStates |
KFNav::matrix | Pstart |
ros::Publisher | pubRange |
ros::Publisher | pubRangeFiltered |
ros::Publisher | pubStateHat |
ros::Publisher | pubStateMeas |
ros::Publisher | pubwk |
bool | quadMeasAvailable |
ros::Subscriber | resetTopic |
KFNav::matrix | Rstart |
ros::Publisher | stateHat |
ros::Publisher | stateMeas |
ros::Subscriber | sub |
ros::Subscriber | subCurrentsHat |
ros::Subscriber | subKFmode |
ros::Subscriber | subStateHat |
ros::Subscriber | subTargetDepth |
ros::Subscriber | subTargetHeading |
ros::Subscriber | subTargetTau |
ros::Subscriber | subUSBL |
ros::Subscriber | subUSBLfix |
ros::Subscriber | tauAch |
KFNav::vector | tauIn |
ros::Publisher | turns_pub |
Turn count publisher. | |
double | u |
ros::Publisher | unsafe_dvl |
labust::math::unwrap | unwrap |
ros::Subscriber | useGyro |
bool | useYawRate |
double | v |
double | xc |
double | yc |
The 3D state estimator for ROVs and AUVs. Extended with altitude and pitch estimates.
The 3D state estimator for ROVs and AUVs. Extended with altitude and pitch estimates.
The 3D state estimator for ROVs and AUVs. Extended with altitude and pitch estimates.
typedef labust::navigation::KFCore<labust::navigation::RelativeTrackingModel> labust::navigation::Estimator3D::KFNav [private] |
Definition at line 72 of file EKF_RTT.hpp.
typedef labust::navigation::KFCore<labust::navigation::EKF_3D_USBLModel> labust::navigation::Estimator3D::KFNav [private] |
Definition at line 79 of file EKF_3D_USBL.hpp.
anonymous enum [private] |
Definition at line 78 of file EKF_3D_USBL.hpp.
anonymous enum [private] |
Definition at line 71 of file EKF_RTT.hpp.
Main constructor.
Main constructor.
int Estimator3D::calculateDelaySteps | ( | double | measTime, |
double | arrivalTime | ||
) | [private] |
Calculate measurement delay in time steps
Definition at line 590 of file EKF_3D_USBL.cpp.
void Estimator3D::configureNav | ( | KFNav & | nav, |
ros::NodeHandle & | nh | ||
) | [private] |
void labust::navigation::Estimator3D::configureNav | ( | KFNav & | nav, |
ros::NodeHandle & | nh | ||
) | [private] |
Helper function for navigation configuration.
void labust::navigation::Estimator3D::configureNav | ( | KFNav & | nav, |
ros::NodeHandle & | nh | ||
) | [private] |
Helper function for navigation configuration.
void Estimator3D::deltaPosCallback | ( | const auv_msgs::NED::ConstPtr & | msg | ) | [private] |
void labust::navigation::Estimator3D::deltaPosCallback | ( | const auv_msgs::NED::ConstPtr & | msg | ) | [private] |
Callbacks for relative/absolute mode switching
void Estimator3D::KFmodeCallback | ( | const std_msgs::Bool::ConstPtr & | msg | ) | [private] |
void labust::navigation::Estimator3D::KFmodeCallback | ( | const std_msgs::Bool::ConstPtr & | msg | ) | [private] |
void Estimator3D::onAltitude | ( | const std_msgs::Float32::ConstPtr & | data | ) | [private] |
void labust::navigation::Estimator3D::onAltitude | ( | const std_msgs::Float32::ConstPtr & | data | ) | [private] |
Handle the altitude measurement.
void Estimator3D::onCurrentsHat | ( | const geometry_msgs::TwistStamped::ConstPtr & | data | ) | [private] |
Handle target input forces and torques.
Definition at line 159 of file EKF_RTT.cpp.
void Estimator3D::onDepth | ( | const std_msgs::Float32::ConstPtr & | data | ) | [private] |
void labust::navigation::Estimator3D::onDepth | ( | const std_msgs::Float32::ConstPtr & | data | ) | [private] |
Handle the depth measurement.
void Estimator3D::onInit | ( | ) |
Initialize the estimation filter.
Initialize the estimation filter.
void labust::navigation::Estimator3D::onModelUpdate | ( | const navcon_msgs::ModelParamsUpdate::ConstPtr & | update | ) | [private] |
On model updates.
void Estimator3D::onModelUpdate | ( | const navcon_msgs::ModelParamsUpdate::ConstPtr & | update | ) | [private] |
void labust::navigation::Estimator3D::onModelUpdate | ( | const navcon_msgs::ModelParamsUpdate::ConstPtr & | update | ) | [private] |
On model updates.
void Estimator3D::onReset | ( | const std_msgs::Bool::ConstPtr & | reset | ) | [private] |
void labust::navigation::Estimator3D::onReset | ( | const std_msgs::Bool::ConstPtr & | reset | ) | [private] |
Handle the state reset.
void Estimator3D::onStateHat | ( | const auv_msgs::NavSts::ConstPtr & | data | ) | [private] |
Handle target input forces and torques.
Definition at line 151 of file EKF_RTT.cpp.
void Estimator3D::onTargetDepth | ( | const std_msgs::Float32::ConstPtr & | data | ) | [private] |
Handle the depth measurement.
Definition at line 172 of file EKF_RTT.cpp.
void Estimator3D::onTargetHeading | ( | const std_msgs::Float32::ConstPtr & | data | ) | [private] |
Handle the depth measurement.
Definition at line 178 of file EKF_RTT.cpp.
void Estimator3D::onTargetTau | ( | const auv_msgs::BodyForceReq::ConstPtr & | tau | ) | [private] |
Handle target input forces and torques.
Definition at line 165 of file EKF_RTT.cpp.
void Estimator3D::onTau | ( | const auv_msgs::BodyForceReq::ConstPtr & | tau | ) | [private] |
void labust::navigation::Estimator3D::onTau | ( | const auv_msgs::BodyForceReq::ConstPtr & | tau | ) | [private] |
Handle input forces and torques.
void labust::navigation::Estimator3D::onUSBLfix | ( | const underwater_msgs::USBLFix::ConstPtr & | data | ) | [private] |
Handle the USBL measurement.
void Estimator3D::onUSBLfix | ( | const underwater_msgs::USBLFix::ConstPtr & | data | ) | [private] |
Handle the USBL measurement.
Definition at line 275 of file EKF_3D_USBL.cpp.
void Estimator3D::onUseGyro | ( | const std_msgs::Bool::ConstPtr & | use_gyro | ) | [private] |
void labust::navigation::Estimator3D::onUseGyro | ( | const std_msgs::Bool::ConstPtr & | use_gyro | ) | [private] |
Handle the gyro/compass switch.
void Estimator3D::processMeasurements | ( | ) | [private] |
void labust::navigation::Estimator3D::processMeasurements | ( | ) | [private] |
Handle the range measurement. Handle the bearing measurement. Helper method to process measurements.
void labust::navigation::Estimator3D::processMeasurements | ( | ) | [private] |
Helper method to process measurements.
void Estimator3D::publishState | ( | ) | [private] |
void labust::navigation::Estimator3D::publishState | ( | ) | [private] |
Helper method to publish the navigation state.
void labust::navigation::Estimator3D::publishState | ( | ) | [private] |
Helper method to publish the navigation state.
Start the estimation loop.
void Estimator3D::start | ( | ) |
Start the estimation loop.
bool labust::navigation::Estimator3D::absoluteEKF [private] |
double labust::navigation::Estimator3D::alt [private] |
double labust::navigation::Estimator3D::compassVariance [private] |
double labust::navigation::Estimator3D::currentTime [private] |
Current time in seconds
Definition at line 223 of file EKF_3D_USBL.hpp.
double labust::navigation::Estimator3D::delay_time [private] |
Fixed time delay for USBL navigation
Definition at line 228 of file EKF_3D_USBL.hpp.
double labust::navigation::Estimator3D::delayTime [private] |
Definition at line 224 of file EKF_3D_USBL.hpp.
float labust::navigation::Estimator3D::deltaXpos [private] |
float labust::navigation::Estimator3D::deltaYpos [private] |
int labust::navigation::Estimator3D::dvl_model [private] |
double labust::navigation::Estimator3D::dvl_timeout [private] |
bool labust::navigation::Estimator3D::enableBearing [private] |
Definition at line 232 of file EKF_3D_USBL.hpp.
bool labust::navigation::Estimator3D::enableDelay [private] |
USBL measurements enable flags
Definition at line 232 of file EKF_3D_USBL.hpp.
bool labust::navigation::Estimator3D::enableElevation [private] |
Definition at line 232 of file EKF_3D_USBL.hpp.
bool labust::navigation::Estimator3D::enableRange [private] |
Definition at line 232 of file EKF_3D_USBL.hpp.
double labust::navigation::Estimator3D::gyroVariance [private] |
The input vector.
Definition at line 143 of file EKF_RTT.hpp.
bool labust::navigation::Estimator3D::KFmode [private] |
bool labust::navigation::Estimator3D::KFmodePast [private] |
double labust::navigation::Estimator3D::max_dvl [private] |
boost::mutex labust::navigation::Estimator3D::meas_mux [private] |
Definition at line 170 of file EKF_3D_USBL.hpp.
double labust::navigation::Estimator3D::min_altitude [private] |
KFNav labust::navigation::Estimator3D::nav [private] |
Definition at line 252 of file EKF_3D_USBL.hpp.
std::deque<FilterState> labust::navigation::Estimator3D::pastStates [private] |
Definition at line 250 of file EKF_3D_USBL.hpp.
Definition at line 178 of file EKF_3D_USBL.hpp.
Definition at line 178 of file EKF_3D_USBL.hpp.
Definition at line 155 of file EKF_RTT.hpp.
Estimated and measured state publisher.
Definition at line 155 of file EKF_RTT.hpp.
Definition at line 178 of file EKF_3D_USBL.hpp.
bool labust::navigation::Estimator3D::quadMeasAvailable [private] |
Definition at line 159 of file EKF_RTT.hpp.
Sensors and input subscribers.
Definition at line 159 of file EKF_RTT.hpp.
Definition at line 159 of file EKF_RTT.hpp.
Definition at line 159 of file EKF_RTT.hpp.
Definition at line 159 of file EKF_RTT.hpp.
Definition at line 182 of file EKF_3D_USBL.hpp.
Definition at line 159 of file EKF_RTT.hpp.
double labust::navigation::Estimator3D::u [private] |
Definition at line 167 of file EKF_RTT.hpp.
bool labust::navigation::Estimator3D::useYawRate [private] |
double labust::navigation::Estimator3D::v [private] |
Definition at line 167 of file EKF_RTT.hpp.
double labust::navigation::Estimator3D::xc [private] |
Definition at line 167 of file EKF_RTT.hpp.
double labust::navigation::Estimator3D::yc [private] |
Definition at line 167 of file EKF_RTT.hpp.