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 #ifndef POSITION_MEASUREMENTS_H
00033 #define POSITION_MEASUREMENTS_H
00034
00035 #include <ros/ros.h>
00036 #include <ssf_core/measurement.h>
00037 #include "position_sensor.h"
00038
00039 class PositionMeasurements: public ssf_core::Measurements
00040 {
00041 public:
00042 PositionMeasurements()
00043 {
00044 addHandler(new PositionSensorHandler(this));
00045
00046 ros::NodeHandle pnh("~");
00047 pnh.param("init/p_ci/x", p_ci_[0], 0.0);
00048 pnh.param("init/p_ci/y", p_ci_[1], 0.0);
00049 pnh.param("init/p_ci/z", p_ci_[2], 0.0);
00050
00051 pnh.param("init/q_ci/w", q_ci_.w(), 1.0);
00052 pnh.param("init/q_ci/x", q_ci_.x(), 0.0);
00053 pnh.param("init/q_ci/y", q_ci_.y(), 0.0);
00054 pnh.param("init/q_ci/z", q_ci_.z(), 0.0);
00055 q_ci_.normalize();
00056
00057 pnh.param("init/q_wv/w", q_wv_.w(), 1.0);
00058 pnh.param("init/q_wv/x", q_wv_.x(), 0.0);
00059 pnh.param("init/q_wv/y", q_wv_.y(), 0.0);
00060 pnh.param("init/q_wv/z", q_wv_.z(), 0.0);
00061 q_wv_.normalize();
00062 }
00063
00064 private:
00065
00066 Eigen::Matrix<double, 3, 1> p_ci_;
00067 Eigen::Quaternion<double> q_ci_;
00068 Eigen::Quaternion<double> q_wv_;
00069
00070 void init(double scale)
00071 {
00072 Eigen::Matrix<double, 3, 1> p, v, b_w, b_a, g, w_m, a_m;
00073 Eigen::Quaternion<double> q;
00074 ssf_core::SSF_Core::ErrorStateCov P;
00075
00076
00077 g << 0, 0, 9.81;
00078 b_w << 0,0,0;
00079 b_a << 0,0,0;
00080
00081 v << 0,0,0;
00082 w_m << 0,0,0;
00083 a_m =g;
00084
00085 P.setZero();
00086
00087
00088 if(p_vc_.norm()==0)
00089 ROS_WARN_STREAM("No measurements received yet to initialize position - using [0 0 0]");
00090 if((q_cv_.norm()==1) & (q_cv_.w()==1))
00091 ROS_WARN_STREAM("No measurements received yet to initialize attitude - using [1 0 0 0]");
00092
00093
00094 q = (q_ci_ * q_cv_.conjugate() * q_wv_).conjugate();
00095 q.normalize();
00096 p = q_wv_.conjugate().toRotationMatrix()*p_vc_/scale - q.toRotationMatrix()*p_ci_;
00097
00098
00099 ssf_core_.initialize(p,v,q,b_w,b_a,scale,q_wv_,P,w_m,a_m,g,q_ci_,p_ci_);
00100
00101 ROS_INFO_STREAM("filter initialized to: \n" <<
00102 "position: [" << p[0] << ", " << p[1] << ", " << p[2] << "]" << std::endl <<
00103 "scale:" << scale << std::endl <<
00104 "attitude (w,x,y,z): [" << q.w() << ", " << q.x() << ", " << q.y() << ", " << q.z() << std::endl <<
00105 "p_ci: [" << p_ci_[0] << ", " << p_ci_[1] << ", " << p_ci_[2] << std::endl <<
00106 "q_ci: (w,x,y,z): [" << q_ci_.w() << ", " << q_ci_.x() << ", " << q_ci_.y() << ", " << q_ci_.z() << "]");
00107 }
00108 };
00109
00110 #endif