00001
00002
00003
00004
00005
00006
00007 #include <math.h>
00008 #include <limits>
00009 #include <StateEstimators/KalmanStateEstimator.hpp>
00010
00011 #include <tk_state/StateEstimatorController.hpp>
00012
00013 #include <telekyb_base/ROS.hpp>
00014
00015 #include <telekyb_msgs/TKState.h>
00016 #include <telekyb_defines/physic_defines.hpp>
00017
00018 PLUGINLIB_DECLARE_CLASS(tk_state, KalmanStateEstimator, telekyb_state::KalmanStateEstimator, TELEKYB_NAMESPACE::StateEstimator);
00019
00020 namespace telekyb_state {
00021
00022 KalmanStateEstimatorOptions::KalmanStateEstimatorOptions()
00023 : OptionContainer("Kalman")
00024 {
00025 imuTopic = addOption<std::string>("imuTopic", "Imu data topic name", "TKImu", false, true);
00026 viconTopic = addOption<std::string>("viconTopic", "Vicon data topic name", "TKVicon", false, true);
00027 stateTopic = addOption<std::string>("stateTopic", "Topic on which the state must be published", "TKKalman", false, true);
00028
00029 Eigen::Matrix<double,6,6> rt;
00030
00031 rt.Zero();
00032 imuCov = addOption<Eigen::Matrix<double,6,6> >("imuCov", "IMU noise covariance matrix", rt, false, true);
00033
00034 imuAccBias = addOption<Eigen::Vector3d>("imuAccBias", "IMU accelerometer bias", Eigen::Vector3d::Zero(), false, true);
00035 imuGyrBias = addOption<Eigen::Vector3d>("imuGyrBias", "IMU gyroscope bias", Eigen::Vector3d::Zero(), false, true);
00036
00037 Eigen::Matrix<double,9,9> initC;
00038
00039 initC.Zero();
00040 initStateCov = addOption<Eigen::Matrix<double,9,9> >("initStateCov", "Initial state covariance estimate", initC, false, true);
00041
00042 vicPosition = addOption<Eigen::Vector3d>("vicPosition", "Initial estimate for Vicon position w.r.t. the IMU", Eigen::Vector3d(0.0,0.0,0.0), false, true);
00043 vicOrientation = addOption<Eigen::Quaterniond>("vicOrientation", "Initial estimate for Vicon orientation w.r.t. the IMU", Eigen::Quaterniond(1.0,0.0,0.0,0.0), false, true);
00044
00045 predStep = addOption<double>("predStep", "Prediction time step", 1e-3, false, true);
00046 minStep = addOption<double>("minStep", "Minimum prediction time step", 1e-3, false, true);
00047 saveStep = addOption<double>("minStep", "Time step for saving states", 1e-3, false, true);
00048
00049 lTpred = addOption<double>("lTpred", "Multiplier for quaternion prediction normalization", 0.8, false, true);
00050
00051 maxStateBufferSize = addOption<int>("maxStateBufferSize", "Maximum number of states to be saved in the state buffer", 200, false, true);
00052 maxMeasureBufferSize = addOption<int>("maxMeasureBufferSize", "Maximum number of measures to be saved in the measure buffer", 200, false, true);
00053 maxInputBufferSize = addOption<int>("maxInputBufferSize", "Maximum number of inputs to be saved in the input buffer", 200, false, true);
00054
00055 }
00056
00057 void KalmanStateEstimator::initialize()
00058 {
00059 isInitialized = false;
00060 isInitializedLinVel = false;
00061 isInitializedPose = false;
00062 isInitializedAngVel = false;
00063
00064 newMeasureReceived = false;
00065 newInputReceived = false;
00066
00067 nodeHandle = ROSModule::Instance().getMainNodeHandle();
00068 }
00069
00070 void KalmanStateEstimator::core()
00071 {
00072 boost::mutex::scoped_lock scopedInputMutex(newInputMutex);
00073 if (newInputReceived)
00074 {
00075
00076 if (inputBuffer.size()==options.maxInputBufferSize->getValue()) inputBuffer.pop_front();
00077
00078
00079 inputBuffer.push_back(newInput);
00080
00081
00082 intTime = Time(newInput.timeStamp);
00083 }
00084 scopedInputMutex.unlock();
00085
00086 boost::mutex::scoped_lock scopedMeasureMutex(newMeasureMutex);
00087 if (newMeasureReceived)
00088 {
00089
00090 std::deque<StateBufferElement>::iterator stateBufferIndex;
00091 std::deque<InputBufferElement>::iterator inputBufferIndex;
00092 std::deque<MeasureBufferElement>::iterator measureBufferIndex;
00093
00094 if (((stateBufferIndex = searchInBuffer<StateBufferElement>(stateBuffer,newMeasure.timeStamp)) < stateBuffer.begin()) ||
00095 ((inputBufferIndex = searchInBuffer<InputBufferElement>(inputBuffer,newMeasure.timeStamp)) < inputBuffer.begin()) ||
00096 ((measureBufferIndex = searchInBuffer<MeasureBufferElement>(measureBuffer,newMeasure.timeStamp)) < measureBuffer.begin()))
00097 {
00098 scopedMeasureMutex.unlock();
00099 ROS_WARN("A measure has been neglected because it was too old. Consider increasing buffer size");
00100 } else {
00101
00102 if ((int)measureBuffer.size()==options.maxMeasureBufferSize->getValue()) measureBuffer.pop_front();
00103 measureBuffer.insert(measureBufferIndex-1,newMeasure);
00104 scopedMeasureMutex.unlock();
00105
00106
00107 stateBuffer.erase(stateBufferIndex,stateBuffer.end());
00108
00109
00110 currentInputIndex = inputBufferIndex;
00111 nextMeasureIndex = measureBufferIndex;
00112 predTime = stateBufferIndex->timeStamp;
00113 saveTimer.reset();
00114 }
00115 }
00116
00117
00118 if (!isInitialized) return;
00119
00120
00121 double nextMeasureTime, nextInputTime, step;
00122 if (nextMeasureIndex > measureBuffer.end())
00123 {
00124 nextMeasureTime = INFINITY;
00125 } else {
00126 nextMeasureTime = nextMeasureIndex->timeStamp;
00127 }
00128
00129 if (currentInputIndex >= inputBuffer.end())
00130 {
00131 nextInputTime = INFINITY;
00132 } else {
00133 nextInputTime = (currentInputIndex+1)->timeStamp;
00134 }
00135
00136 bool done = false;
00137 while (!done)
00138 {
00139 int condition = tools.min(Eigen::Vector4d(options.predStep->getValue(),
00140 intTime.toDSec()-predTime,
00141 nextInputTime-predTime,
00142 nextMeasureTime-predTime), step);
00143 if (step>=options.minStep->getValue())
00144 {
00145 prediction(internalState,*currentInputIndex,step);
00146 }
00147 predTime += step;
00148
00149 switch (condition)
00150 {
00151 case (1):
00152 done = true;
00153 break;
00154 case (2):
00155 currentInputIndex++;
00156 if (currentInputIndex == inputBuffer.end())
00157 {
00158 nextInputTime = INFINITY;
00159 } else {
00160 nextInputTime = (currentInputIndex+1)->timeStamp;
00161 }
00162 break;
00163
00164 case (3):
00165 viconHandler.update(internalState,*nextMeasureIndex.measure);
00166 nextMeasureIndex++;
00167 if (nextMeasureIndex == measureBuffer.end())
00168 {
00169 nextMeasureTime = INFINITY;
00170 } else {
00171 nextMeasureTime = (nextMeasureIndex)->timeStamp;
00172 }
00173 break;
00174 default:
00175 ROS_ERROR("Unexpected condition");
00176 break;
00177 }
00178
00179 if (saveTimer.getElapsed() >= options.saveStep->getValue())
00180 {
00181 stateBuffer.push_back(internalState);
00182 saveTimer.reset();
00183 }
00184 }
00185
00186
00187 publishEstimate(internalState);
00188 }
00189
00190 void KalmanStateEstimator::publishEstimate(const StateBufferElement & estimate)
00191 {
00192 statePub.publish(estimate.state);
00193 return;
00194 }
00195
00196
00197 void KalmanStateEstimator::imuCallback(const sensor_msgs::Imu::ConstPtr& msg)
00198 {
00199 boost::mutex::scoped_lock scopedLockMutex(newInputMutex);
00200
00201 newInput.timeStamp = msg->header.stamp.sec;
00202 newInput.input = *msg;
00203 newInputReceived = true;
00204
00205 if (!isInitialized)
00206 {
00207 if (!isInitializedAngVel)
00208 {
00209 internalState.state.twist.angular = msg->angular_velocity;
00210 isInitializedAngVel = true;
00211 }
00212 if (isInitializedLinVel)
00213 {
00214 internalState.covariance = options.initStateCov->getValue();
00215 isInitialized = true;
00216 }
00217 }
00218
00219 scopedLockMutex.unlock();
00220 }
00221
00222
00223 void KalmanStateEstimator::viconCallback(const geometry_msgs::TransformStamped::ConstPtr& msg)
00224 {
00225 boost::mutex::scoped_lock scopedLockMutex(newMeasureMutex);
00226 MeasureBufferElement measure;
00227 newMeasure.timeStamp = msg->header.stamp.sec;
00228 newMeasure.measure = *msg;
00229 newMeasureReceived = true;
00230 if(!isInitialized)
00231 {
00232 if (!isInitializedLinVel)
00233 {
00234 if(!isInitializedPose)
00235 {
00236 internalState.state.pose.position.x = msg->transform.translation.x;
00237 internalState.state.pose.position.y = msg->transform.translation.y;
00238 internalState.state.pose.position.z = msg->transform.translation.z;
00239
00240 internalState.state.pose.orientation.w = msg->transform.rotation.w;
00241 internalState.state.pose.orientation.x = msg->transform.rotation.x;
00242 internalState.state.pose.orientation.y = msg->transform.rotation.y;
00243 internalState.state.pose.orientation.z = msg->transform.rotation.z;
00244
00245 internalState.timeStamp = msg->header.stamp.toSec();
00246 internalState.state.header.stamp.fromSec(internalState.timeStamp);
00247 isInitializedPose = true;
00248 } else {
00249 internalState.state.twist.linear.x = (msg->transform.translation.x - internalState.state.pose.position.x)/(msg->header.stamp.toSec()-internalState.timeStamp);
00250 internalState.state.twist.linear.y = (msg->transform.translation.y - internalState.state.pose.position.y)/(msg->header.stamp.toSec()-internalState.timeStamp);
00251 internalState.state.twist.linear.z = (msg->transform.translation.z - internalState.state.pose.position.z)/(msg->header.stamp.toSec()-internalState.timeStamp);
00252 isInitializedLinVel = true;
00253 }
00254 } else if (isInitializedAngVel)
00255 isInitialized = true;
00256 }
00257
00258 scopedLockMutex.unlock();
00259 }
00260
00261
00262 void KalmanStateEstimator::prediction(StateBufferElement& state, const InputBufferElement& u, double Tpred)
00263 {
00264 double lTpred = options.lTpred->getValue();
00265
00266
00267 Eigen::Vector3d r(state.state.pose.position.x,state.state.pose.position.y,state.state.pose.position.z);
00268 Eigen::Vector3d dr(state.state.twist.linear.x,state.state.twist.linear.y,state.state.twist.linear.z);
00269 Eigen::Quaterniond q(state.state.pose.orientation.w,state.state.pose.orientation.x,state.state.pose.orientation.y,state.state.pose.orientation.z);
00270
00271
00272 Eigen::Vector3d w(u.input.angular_velocity.x,u.input.angular_velocity.y,u.input.angular_velocity.z);
00273 Eigen::Vector3d a(u.input.linear_acceleration.x,u.input.linear_acceleration.y,u.input.linear_acceleration.z);
00274
00275 w -= options.imuGyrBias->getValue();
00276 a -= options.imuAccBias->getValue();
00277
00278
00279 Eigen::Matrix4d qqT;
00280 qqT(0) = q.w()*q.w();
00281 qqT.block<3,3>(1,1) = q.vec()*q.vec().transpose();
00282 qqT.block<3,1>(0,1) = q.w()*q.vec();
00283 qqT.block<1,3>(1,0) = qqT.block<3,1>(0,1).transpose();
00284 double qTq = qqT.trace();
00285
00286 double s = .5*w.norm()*Tpred;
00287
00288 double sss,f3;
00289 if (abs(s)<std::numeric_limits<double>::epsilon())
00290 {
00291 sss = 1;
00292 f3 = -1/3*pow(.5*Tpred,3);
00293 } else {
00294 sss = sin(s)/s;
00295 f3 = pow(.5*Tpred,3)*(cos(s)-sss)/pow(s,2);
00296 }
00297
00298 double k = 1-qTq;
00299 double f1 = cos(s)+lTpred*k;
00300 double f2 = .5*Tpred*sss;
00301
00302 r += dr*Tpred;
00303 dr += (q*a-Eigen::Vector3d(0,0,GRAVITY))*Tpred;
00304 q = tools.toQuaternion((Eigen::Vector4d)(f1*tools.toVector(q) + f2*tools.toVector(Eigen::Quaterniond(0.0,w(1),w(2),w(3))*q)));
00305
00306 state.state.pose.position.x = r(0);
00307 state.state.pose.position.y = r(1);
00308 state.state.pose.position.z = r(2);
00309
00310 state.state.twist.linear.x = dr(0);
00311 state.state.twist.linear.y = dr(1);
00312 state.state.twist.linear.z = dr(2);
00313
00314 state.state.pose.orientation.w = q.w();
00315 state.state.pose.orientation.x = q.vec()(0);
00316 state.state.pose.orientation.y = q.vec()(1);
00317 state.state.pose.orientation.z = q.vec()(2);
00318
00319 state.state.twist.angular.x = w(0);
00320 state.state.twist.angular.y = w(1);
00321 state.state.twist.angular.z = w(2);
00322
00323
00324 Eigen::Matrix<double, 9, 9> G;
00325 G.Zero();
00326
00327 G.block<3,3>(0,0).Identity();
00328 G.block<3,3>(0,3) = Tpred*Eigen::Matrix3d::Identity();
00329
00330 G.block<3,3>(3,3).Identity();
00331 G.block<3,3>(3,6) = Tpred*tools.gamma(q,a)*tools.jacqp(q);
00332
00333 Eigen::Matrix4d jacqq;
00334 jacqq = f1*Eigen::Matrix4d::Identity() + f2*tools.qLeft(Eigen::Quaterniond(0,w(0),w(1),w(2))) - 2*lTpred*qqT;
00335 G.block<3,3>(6,6) = tools.jacpq(q)*jacqq*tools.jacqp(q);
00336
00337
00338
00339 Eigen::Matrix<double, 4, 3> T;
00340 T = tools.matW(q)*(f2*Eigen::Matrix3d::Identity()+f3*(w*w.transpose()))-.5*Tpred*f2*tools.toVector(q)*w.transpose();
00341
00342 Eigen::Matrix<double, 9, 6> F;
00343 F << Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Zero(),
00344 Eigen::Matrix3d::Zero(), Tpred*q.toRotationMatrix(),
00345 tools.jacpq(q)*T, Eigen::Matrix3d::Zero();
00346
00347
00348 state.covariance = G*state.covariance*G.transpose() + (F*options.imuCov->getValue()*F.transpose() + options.discCov->getValue())*Tpred;
00349
00350 state.timeStamp = state.state.header.stamp.toSec() + Tpred;
00351 state.state.header.stamp.fromSec(state.timeStamp);
00352 }
00353
00354
00355 void KalmanStateEstimator::willBecomeActive()
00356 {
00357 imuSub = nodeHandle.subscribe(options.imuTopic->getValue(),1, &KalmanStateEstimator::imuCallback, this);
00358 statePub = nodeHandle.advertise<telekyb_msgs::TKState>(options.stateTopic->getValue(),1);
00359 }
00360
00361
00362 void KalmanStateEstimator::willBecomeInActive()
00363 {
00364 imuSub.shutdown();
00365 }
00366
00367 void KalmanStateEstimator::destroy(){}
00368
00369 std::string KalmanStateEstimator::getName() const
00370 {
00371 return options.viconTopic->getValue();
00372 }
00373
00374 }