00001
00022 #include "DroneKalmanFilter.h"
00023 #include "EstimationNode.h"
00024
00025
00026 const double varSpeedObservation_xy = 2*2;
00027 const double varPoseObservation_xy = 0.2*0.2;
00028 const double varAccelerationError_xy = 8*8;
00029
00030 const double varPoseObservation_z_PTAM = 0.08*0.08;
00031 const double varPoseObservation_z_IMU = 0.25*0.25;
00032 const double varPoseObservation_z_IMU_NO_PTAM = 0.1*0.1;
00033 const double varAccelerationError_z = 1*1;
00034
00035 const double varPoseObservation_rp_PTAM = 3*3;
00036 const double varPoseObservation_rp_IMU = 1*1;
00037 const double varSpeedError_rp = 360*360 * 16;
00038
00039 const double varSpeedObservation_yaw = 5*5;
00040 const double varPoseObservation_yaw = 3*3;
00041 const double varAccelerationError_yaw = 360*360;
00042
00043
00044
00045
00046 int DroneKalmanFilter::delayRPY = 0;
00047 int DroneKalmanFilter::delayXYZ = 40;
00048 int DroneKalmanFilter::delayVideo = 75;
00049 int DroneKalmanFilter::delayControl = 100;
00050
00051 const int DroneKalmanFilter::base_delayXYZ = 40;
00052 const int DroneKalmanFilter::base_delayVideo = 50;
00053 const int DroneKalmanFilter::base_delayControl = 50;
00054
00055
00056 const double max_z_speed = 2.5;
00057 const double scaleUpdate_min_xyDist = 0.5*0.5*0.5*0.5;
00058 const double scaleUpdate_min_zDist = 0.1*0.1;
00059
00060 using namespace std;
00061
00062
00063 double angleFromTo(double angle, double min, double sup)
00064 {
00065 while(angle < min) angle += 360;
00066 while(angle >= sup) angle -= 360;
00067 return angle;
00068 }
00069
00070
00071
00072
00073
00074 pthread_mutex_t DroneKalmanFilter::filter_CS = PTHREAD_MUTEX_INITIALIZER;
00075
00076 DroneKalmanFilter::DroneKalmanFilter(EstimationNode* n)
00077 {
00078 scalePairs = new std::vector<ScaleStruct>();
00079 navdataQueue = new std::deque<ardrone_autonomy::Navdata>();
00080 velQueue = new std::deque<geometry_msgs::TwistStamped>();
00081
00082 useScalingFixpoint = false;
00083
00084 this->node = n;
00085
00086 pthread_mutex_lock( &filter_CS );
00087 reset();
00088 pthread_mutex_unlock( &filter_CS );
00089
00090
00091
00092
00093
00094 c1 = 0.58;
00095 c2 = 17.8;
00096 c3 = 10;
00097 c4 = 35;
00098 c5 = 10;
00099 c6 = 25;
00100 c7 = 1.4;
00101 c8 = 1.0;
00102
00103 }
00104
00105
00106 DroneKalmanFilter::~DroneKalmanFilter(void)
00107 {
00108
00109 }
00110
00111 void DroneKalmanFilter::release()
00112 {
00113 delete scalePairs;
00114 delete navdataQueue;
00115 delete velQueue;
00116 }
00117
00118
00119 void DroneKalmanFilter::setPing(unsigned int navPing, unsigned int vidPing)
00120 {
00121
00122
00123 navPing += 20;
00124 vidPing += 40;
00125
00126 int new_delayXYZ = base_delayXYZ;
00127 int new_delayVideo = base_delayVideo + vidPing/(int)2 - navPing/(int)2;
00128 int new_delayControl = base_delayControl + navPing;
00129
00130 delayXYZ = std::min(500,std::max(40,std::min(new_delayVideo,new_delayXYZ)));
00131 delayVideo = std::min(500,std::max(40,new_delayVideo));
00132 delayControl = std::min(200,std::max(50,new_delayControl));
00133
00134 std::cout << "new delasXYZ: " << delayXYZ << ", delayVideo: " << delayVideo << ", delayControl: " << delayControl << std::endl;
00135 }
00136
00137 void DroneKalmanFilter::reset()
00138 {
00139
00140 x = y = z = yaw = PVFilter(0);
00141 roll = pitch = PFilter(0);
00142 lastIMU_XYZ_ID = lastIMU_RPY_ID = -1;
00143 predictedUpToDroneTime = 0;
00144 last_z_heightDiff = 0;
00145 scalePairsIn = scalePairsOut = 0;
00146
00147
00148 numGoodIMUObservations = 0;
00149
00150
00151 lastIMU_XYZ_dronetime = lastIMU_RPY_dronetime = 0;
00152 lastIMU_dronetime = 0;
00153
00154
00155 clearPTAM();
00156
00157
00158 navdataQueue->clear();
00159 velQueue->clear();
00160
00161 predictdUpToTimestamp = getMS(ros::Time::now());
00162 predictedUpToTotal = -1;
00163
00164 baselineZ_Filter = baselineZ_IMU = -999999;
00165 baselinesYValid = false;
00166
00167
00168 node->publishCommand("u l EKF has been reset to zero.");
00169 }
00170
00171 void DroneKalmanFilter::clearPTAM()
00172 {
00173
00174 offsets_xyz_initialized = scale_xyz_initialized = false;
00175 xy_scale = z_scale = scale_from_xy = scale_from_z = 1;
00176 roll_offset = pitch_offset = yaw_offset = x_offset = y_offset = z_offset = 0;
00177
00178 xyz_sum_IMUxIMU = 0.1;
00179 xyz_sum_PTAMxPTAM = 0.1;
00180 xyz_sum_PTAMxIMU = 0.1;
00181 scalePairs->clear();
00182 scalePairsIn = 1;
00183 scalePairsOut = 0;
00184
00185 rp_offset_framesContributed = 0;
00186
00187
00188 numGoodPTAMObservations = 0;
00189
00190
00191 lastPosesValid = false;
00192 }
00193
00194
00195
00196
00197 void DroneKalmanFilter::predictInternal(geometry_msgs::Twist activeControlInfo, int timeSpanMicros, bool useControlGains)
00198 {
00199 if(timeSpanMicros <= 0) return;
00200
00201 useControlGains = useControlGains && this->useControl;
00202
00203 bool controlValid = !(activeControlInfo.linear.z > 1.01 || activeControlInfo.linear.z < -1.01 ||
00204 activeControlInfo.linear.x > 1.01 || activeControlInfo.linear.x < -1.01 ||
00205 activeControlInfo.linear.y > 1.01 || activeControlInfo.linear.y < -1.01 ||
00206 activeControlInfo.angular.z > 1.01 || activeControlInfo.angular.z < -1.01);
00207
00208
00209 double tsMillis = timeSpanMicros / 1000.0;
00210 double tsSeconds = tsMillis / 1000.0;
00211
00212
00213
00214 float rollControlGain = tsSeconds*c3*(c4 * max(-0.5, min(0.5, (double)activeControlInfo.linear.y)) - roll.state);
00215 float pitchControlGain = tsSeconds*c3*(c4 * max(-0.5, min(0.5, (double)activeControlInfo.linear.x)) - pitch.state);
00216 float yawSpeedControlGain = tsSeconds*c5*(c6 * activeControlInfo.angular.z - yaw.state[1]);
00217
00218
00219
00220 double yawRad = yaw.state[0] * 3.14159268 / 180;
00221 double rollRad = roll.state * 3.14159268 / 180;
00222 double pitchRad = pitch.state * 3.14159268 / 180;
00223 double forceX = cos(yawRad) * sin(rollRad) * cos(pitchRad) - sin(yawRad) * sin(pitchRad);
00224 double forceY = - sin(yawRad) * sin(rollRad) * cos(pitchRad) - cos(yawRad) * sin(pitchRad);
00225
00226
00227 double vx_gain = tsSeconds * c1 * (c2*forceX - x.state[1]);
00228 double vy_gain = tsSeconds * c1 * (c2*forceY - y.state[1]);
00229 double vz_gain = tsSeconds * c7 * (c8*activeControlInfo.linear.z*(activeControlInfo.linear.z < 0 ? 2 : 1) - z.state[1]);
00230
00231 lastVXGain = vx_gain;
00232 lastVYGain = vy_gain;
00233 lastPredictedRoll = roll.state;
00234 lastPredictedPitch = pitch.state;
00235
00236 if(!useControlGains || !controlValid)
00237 {
00238 vx_gain = vy_gain = vz_gain = 0;
00239 rollControlGain = pitchControlGain = yawSpeedControlGain = 0;
00240 }
00241
00242
00243 yaw.state[0] = angleFromTo(yaw.state[0],-180,180);
00244 roll.predict(tsMillis,varSpeedError_rp, rollControlGain);
00245 pitch.predict(tsMillis,varSpeedError_rp, pitchControlGain);
00246 yaw.predict(tsMillis,varAccelerationError_yaw,TooN::makeVector(tsSeconds*yawSpeedControlGain/2,yawSpeedControlGain),1,5*5);
00247 yaw.state[0] = angleFromTo(yaw.state[0],-180,180);
00248
00249 x.predict(tsMillis,varAccelerationError_xy,TooN::makeVector(tsSeconds*vx_gain/2,vx_gain),0.0001);
00250 y.predict(tsMillis,varAccelerationError_xy,TooN::makeVector(tsSeconds*vy_gain/2,vy_gain),0.0001);
00251 z.predict(tsMillis,TooN::makeVector(tsSeconds*tsSeconds*tsSeconds*tsSeconds, 9*tsSeconds,tsSeconds*tsSeconds*tsSeconds*3),
00252 TooN::makeVector(tsSeconds*vz_gain/2,vz_gain));
00253 }
00254
00255
00256
00257 void DroneKalmanFilter::observeIMU_XYZ(const ardrone_autonomy::Navdata* nav)
00258 {
00259
00260
00261
00262 double yawRad = yaw.state[0] * 3.14159268 / 180;
00263 double vx_global = (sin(yawRad) * nav->vx + cos(yawRad) * nav->vy) / 1000.0;
00264 double vy_global = (cos(yawRad) * nav->vx - sin(yawRad) * nav->vy) / 1000.0;
00265
00266 if(vx_global > 10)
00267 cout << "err";
00268
00269
00270
00271
00272 double lastX = x.state[0];
00273 double lastY = y.state[0];
00274 if(lastPosesValid)
00275 {
00276 x.observeSpeed(vx_global,varSpeedObservation_xy*50);
00277 y.observeSpeed(vy_global,varSpeedObservation_xy*50);
00278 }
00279 else
00280 {
00281 x.observeSpeed(vx_global,varSpeedObservation_xy);
00282 y.observeSpeed(vy_global,varSpeedObservation_xy);
00283 }
00284
00285
00286
00287 if(abs(lastX-x.state[0]) > 0.2 && lastX != 0)
00288 {
00289
00290 ROS_WARN("detected large x jump. removing. should not happen usually (only e.g. if no navdata for a long time, or agressive re-scaling)");
00291 x.state[0] = lastX;
00292 }
00293 if(abs(lastY-y.state[0]) > 0.2 && lastY != 0)
00294 {
00295 y.state[0] = lastY;
00296 ROS_WARN("detected large y jump. removing. should not happen usually (only e.g. if no navdata for a long time, or agressive re-scaling)");
00297 }
00298
00299
00300
00301 if(last_z_IMU != nav->altd || nav->header.seq - last_z_packageID > 8)
00302 {
00303 if(baselineZ_Filter < -100000)
00304 {
00305 baselineZ_IMU = nav->altd;
00306 baselineZ_Filter = z.state[0];
00307 }
00308
00309
00310
00311 if(lastPosesValid)
00312 {
00313
00314 double imuHeightDiff = (nav->altd - baselineZ_IMU )*0.001;
00315 double observedHeight = baselineZ_Filter + 0.5*(imuHeightDiff + last_z_heightDiff);
00316 last_z_heightDiff = imuHeightDiff;
00317
00318 baselineZ_IMU = nav->altd;
00319 baselineZ_Filter = z.state[0];
00320
00321 if((abs(imuHeightDiff) < 0.150 && abs(last_z_heightDiff) < 0.150))
00322 {
00323 z.observePose(observedHeight,varPoseObservation_z_IMU);
00324 lastdZ = observedHeight;
00325 }
00326 }
00327 else
00328 {
00329 double imuHeightDiff = (nav->altd - baselineZ_IMU )*0.001;
00330 double observedHeight = baselineZ_Filter + imuHeightDiff;
00331
00332 if(abs(imuHeightDiff) < 0.110)
00333 {
00334 z.observePose(observedHeight,varPoseObservation_z_IMU_NO_PTAM);
00335 lastdZ = observedHeight;
00336 }
00337 else
00338 {
00339 if(baselineZ_IMU == 0 || nav->altd == 0)
00340 {
00341 z.observePose(observedHeight,0);
00342 z.observeSpeed(0,0);
00343 }
00344
00345 baselineZ_IMU = nav->altd;
00346 baselineZ_Filter = z.state[0];
00347 }
00348 }
00349
00350 last_z_IMU = nav->altd;
00351 last_z_packageID = nav->header.seq;
00352 }
00353
00354 }
00355
00356
00357
00358 void DroneKalmanFilter::observeIMU_RPY(const ardrone_autonomy::Navdata* nav)
00359 {
00360 roll.observe(nav->rotX,varPoseObservation_rp_IMU);
00361 pitch.observe(nav->rotY,varPoseObservation_rp_IMU);
00362
00363
00364 if(!baselinesYValid)
00365 {
00366 baselineY_IMU = nav->rotZ;
00367 baselineY_Filter = yaw.state[0];
00368 baselinesYValid = true;
00369 timestampYawBaselineFrom = getMS(nav->header.stamp);
00370 }
00371
00372 double imuYawDiff = (nav->rotZ - baselineY_IMU );
00373 double observedYaw = baselineY_Filter + imuYawDiff;
00374
00375 yaw.state[0] = angleFromTo(yaw.state[0],-180,180);
00376
00377 if(yaw.state[0] < -90)
00378 observedYaw = angleFromTo(observedYaw,-360,0);
00379 else if(yaw.state[0] > 90)
00380 observedYaw = angleFromTo(observedYaw,0,360);
00381 else
00382 observedYaw = angleFromTo(observedYaw,-180,180);
00383
00384
00385 if(lastPosesValid)
00386 {
00387
00388 baselineY_IMU = nav->rotZ;
00389 baselineY_Filter = yaw.state[0];
00390 timestampYawBaselineFrom = getMS(nav->header.stamp);
00391
00392
00393 if(abs(observedYaw - yaw.state[0]) < 10)
00394 {
00395 yaw.observePose(observedYaw,2*2);
00396 lastdYaw = observedYaw;
00397 }
00398 }
00399 else
00400 if(abs(observedYaw - yaw.state[0]) < 10)
00401 {
00402 yaw.observePose(observedYaw,1*1);
00403 lastdYaw = observedYaw;
00404 }
00405 else
00406 {
00407 baselineY_IMU = nav->rotZ;
00408 baselineY_Filter = yaw.state[0];
00409 timestampYawBaselineFrom = getMS(nav->header.stamp);
00410 }
00411
00412 last_yaw_IMU = nav->rotZ;
00413 yaw.state[0] = angleFromTo(yaw.state[0],-180,180);
00414 }
00415
00416
00417
00418 void DroneKalmanFilter::observePTAM(TooN::Vector<6> pose)
00419 {
00420
00421
00422
00423 if(!allSyncLocked)
00424 {
00425 sync_xyz(pose[0], pose[1], pose[2]);
00426 sync_rpy(pose[3], pose[4], pose[5]);
00427 }
00428
00429 last_PTAM_pose = TooN::makeVector(pose[0], pose[1], pose[2]);
00430
00431
00432 pose.slice<0,3>() = transformPTAMObservation(pose[0], pose[1], pose[2]);
00433
00434 if(offsets_xyz_initialized)
00435 {
00436 x.observePose(pose[0],varPoseObservation_xy);
00437 y.observePose(pose[1],varPoseObservation_xy);
00438 }
00439
00440
00441 if(offsets_xyz_initialized)
00442 {
00443 z.observePose(pose[2], varPoseObservation_z_PTAM);
00444 }
00445
00446
00447 if(rp_offset_framesContributed > 1)
00448 {
00449 roll.observe(pose[3]+roll_offset,varPoseObservation_rp_PTAM);
00450 pitch.observe(pose[4]+pitch_offset,varPoseObservation_rp_PTAM);
00451
00452 yaw.state[0] = angleFromTo(yaw.state[0],-180,180);
00453 double observedYaw = pose[5]+yaw_offset;
00454
00455 if(yaw.state[0] < -90)
00456 observedYaw = angleFromTo(observedYaw,-360,0);
00457 else if(yaw.state[0] > 90)
00458 observedYaw = angleFromTo(observedYaw,0,360);
00459 else
00460 observedYaw = angleFromTo(observedYaw,-180,180);
00461
00462 yaw.observePose(observedYaw,varPoseObservation_yaw);
00463 yaw.state[0] = angleFromTo(yaw.state[0],-180,180);
00464 }
00465
00466 last_fused_pose = TooN::makeVector(x.state[0], y.state[0], z.state[0]);
00467 lastPosesValid = true;
00468 }
00469
00470
00471 void DroneKalmanFilter::sync_rpy(double roll_global, double pitch_global, double yaw_global)
00472 {
00473 if(allSyncLocked) return;
00474
00475 if(rp_offset_framesContributed < 1)
00476 yaw_offset = yaw.state[0] - yaw_global;
00477
00478
00479 if(rp_offset_framesContributed < 100)
00480 {
00481 roll_offset = rp_offset_framesContributed * roll_offset + roll.state - roll_global;
00482 pitch_offset = rp_offset_framesContributed * pitch_offset + pitch.state - pitch_global;
00483 }
00484 rp_offset_framesContributed++;
00485
00486 roll_offset /= rp_offset_framesContributed;
00487 pitch_offset /= rp_offset_framesContributed;
00488 }
00489
00490 void DroneKalmanFilter::sync_xyz(double x_global, double y_global, double z_global)
00491 {
00492 if(allSyncLocked) return;
00493
00494 if(!offsets_xyz_initialized)
00495 {
00496 x_offset = x.state[0] - x_global*xy_scale;
00497 y_offset = y.state[0] - y_global*xy_scale;
00498 z_offset = z.state[0] - z_global*z_scale;
00499 offsets_xyz_initialized = true;
00500 }
00501
00502
00503 }
00504
00505
00506
00507 void DroneKalmanFilter::flushScalePairs()
00508 {
00509 std::ofstream* fle = new std::ofstream();
00510 fle->open ("scalePairs.txt");
00511 for(unsigned int i=0;i<scalePairs->size();i++)
00512 (*fle) << (*scalePairs)[i].ptam[0] << " " <<(*scalePairs)[i].ptam[1] << " " <<(*scalePairs)[i].ptam[2] << " " <<
00513 (*scalePairs)[i].imu[0] << " " << (*scalePairs)[i].imu[1] << " " << (*scalePairs)[i].imu[2] << std::endl;
00514 fle->flush();
00515 fle->close();
00516 delete fle;
00517 }
00518
00519 void DroneKalmanFilter::updateScaleXYZ(TooN::Vector<3> ptamDiff, TooN::Vector<3> imuDiff, TooN::Vector<3> OrgPtamPose)
00520 {
00521 if(allSyncLocked) return;
00522
00523 ScaleStruct s = ScaleStruct(ptamDiff, imuDiff);
00524
00525
00526 if(s.imuNorm < 0.05 || s.ptamNorm < 0.05) return;
00527
00528
00529
00530 (*scalePairs).push_back(s);
00531
00532 double xyz_scale_old = xy_scale;
00533
00534
00535
00536 std::sort((*scalePairs).begin(), (*scalePairs).end());
00537 double median = (*scalePairs)[((*scalePairs).size()+1)/2].alphaSingleEstimate;
00538
00539
00540
00541 if((*scalePairs).size() < 5)
00542 median = initialScaleSet;
00543
00544
00545
00546 double sumII = 0;
00547 double sumPP = 0;
00548 double sumPI = 0;
00549 double totSumII = 0;
00550 double totSumPP = 0;
00551 double totSumPI = 0;
00552
00553 double sumIIxy = 0;
00554 double sumPPxy = 0;
00555 double sumPIxy = 0;
00556 double sumIIz = 0;
00557 double sumPPz = 0;
00558 double sumPIz = 0;
00559
00560 int numIn = 0;
00561 int numOut = 0;
00562 for(unsigned int i=0;i<(*scalePairs).size();i++)
00563 {
00564 if((*scalePairs).size() < 5 || ((*scalePairs)[i].alphaSingleEstimate > median * 0.2 && (*scalePairs)[i].alphaSingleEstimate < median / 0.2))
00565 {
00566 sumII += (*scalePairs)[i].ii;
00567 sumPP += (*scalePairs)[i].pp;
00568 sumPI += (*scalePairs)[i].pi;
00569
00570 sumIIxy += (*scalePairs)[i].imu[0]*(*scalePairs)[i].imu[0] + (*scalePairs)[i].imu[1]*(*scalePairs)[i].imu[1];
00571 sumPPxy += (*scalePairs)[i].ptam[0]*(*scalePairs)[i].ptam[0] + (*scalePairs)[i].ptam[1]*(*scalePairs)[i].ptam[1];
00572 sumPIxy += (*scalePairs)[i].ptam[0]*(*scalePairs)[i].imu[0] + (*scalePairs)[i].ptam[1]*(*scalePairs)[i].imu[1];
00573
00574 sumIIz += (*scalePairs)[i].imu[2]*(*scalePairs)[i].imu[2];
00575 sumPPz += (*scalePairs)[i].ptam[2]*(*scalePairs)[i].ptam[2];
00576 sumPIz += (*scalePairs)[i].ptam[2]*(*scalePairs)[i].imu[2];
00577
00578 numIn++;
00579 }
00580 else
00581 {
00582 totSumII += (*scalePairs)[i].ii;
00583 totSumPP += (*scalePairs)[i].pp;
00584 totSumPI += (*scalePairs)[i].pi;
00585 numOut++;
00586 }
00587 }
00588 xyz_sum_IMUxIMU = sumII;
00589 xyz_sum_PTAMxPTAM = sumPP;
00590 xyz_sum_PTAMxIMU = sumPI;
00591
00592 double scale_Filtered = (*scalePairs)[0].computeEstimator(sumPP,sumII,sumPI,0.2,0.01);
00593 double scale_Unfiltered = (*scalePairs)[0].computeEstimator(sumPP+totSumPP,sumII+totSumII,sumPI+totSumPI,0.2,0.01);
00594 double scale_PTAMSmallVar = (*scalePairs)[0].computeEstimator(sumPP+totSumPP,sumII+totSumII,sumPI+totSumPI,0.00001,1);
00595 double scale_IMUSmallVar = (*scalePairs)[0].computeEstimator(sumPP+totSumPP,sumII+totSumII,sumPI+totSumPI,1,0.00001);
00596
00597
00598 double scale_Filtered_xy = (*scalePairs)[0].computeEstimator(sumPPxy,sumIIxy,sumPIxy,0.2,0.01);
00599 double scale_Filtered_z = (*scalePairs)[0].computeEstimator(sumPPz,sumIIz,sumPIz,0.2,0.01);
00600
00601
00602 scalePairsIn = numIn;
00603 scalePairsOut = numOut;
00604
00605 printf("scale: in: %i; out: %i, filt: %.3f; xyz: %.1f < %.1f < %.1f; xy: %.1f < %.1f < %.1f; z: %.1f < %.1f < %.1f;\n",
00606 numIn, numOut, scale_Filtered,
00607 scale_PTAMSmallVar, scale_Unfiltered, scale_IMUSmallVar,
00608 (*scalePairs)[0].computeEstimator(sumPPxy,sumIIxy,sumPIxy,0.00001,1),
00609 scale_Filtered_xy,
00610 (*scalePairs)[0].computeEstimator(sumPPxy,sumIIxy,sumPIxy,1,0.00001),
00611 (*scalePairs)[0].computeEstimator(sumPPz,sumIIz,sumPIz,0.00001,1),
00612 scale_Filtered_z,
00613 (*scalePairs)[0].computeEstimator(sumPPz,sumIIz,sumPIz,1,0.00001)
00614 );
00615
00616
00617 if(scale_Filtered > 0.1)
00618 z_scale = xy_scale = scale_Filtered;
00619 else
00620 ROS_WARN("calculated scale is too small %.3f, disallowing!",scale_Filtered);
00621
00622
00623 scale_from_xy = scale_Filtered_xy;
00624 scale_from_z = scale_Filtered_z;
00625
00626 if(useScalingFixpoint)
00627 {
00628
00629 x_offset += (xyz_scale_old - xy_scale)*scalingFixpoint[0];
00630 y_offset += (xyz_scale_old - xy_scale)*scalingFixpoint[1];
00631 z_offset += (xyz_scale_old - z_scale)*scalingFixpoint[2];
00632 }
00633 else
00634 {
00635
00636 x_offset += (xyz_scale_old - xy_scale)*OrgPtamPose[0];
00637 y_offset += (xyz_scale_old - xy_scale)*OrgPtamPose[1];
00638 z_offset += (xyz_scale_old - z_scale)*OrgPtamPose[2];
00639 }
00640 scale_xyz_initialized = true;
00641 }
00642
00643 float DroneKalmanFilter::getScaleAccuracy()
00644 {
00645 return 0.5 + 0.5*std::min(1.0,std::max(0.0,xyz_sum_PTAMxIMU * xy_scale/4));
00646 }
00647
00648
00649 void DroneKalmanFilter::predictUpTo(int timestamp, bool consume, bool useControlGains)
00650 {
00651 if(predictdUpToTimestamp == timestamp) return;
00652
00653
00654
00655
00656
00657
00658
00659
00660
00661
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672 std::deque<geometry_msgs::TwistStamped>::iterator controlIterator = velQueue->begin();
00673 while(controlIterator != velQueue->end() &&
00674 controlIterator+1 != velQueue->end() &&
00675 getMS((controlIterator+1)->header.stamp) <= predictdUpToTimestamp - delayControl)
00676 if(consume)
00677 {
00678 velQueue->pop_front();
00679 controlIterator = velQueue->begin();
00680 }
00681 else
00682 controlIterator++;
00683 if(velQueue->size() == 0) useControlGains = false;
00684
00685
00686 std::deque<ardrone_autonomy::Navdata>::iterator xyzIterator = navdataQueue->begin();
00687 while(xyzIterator != navdataQueue->end() &&
00688 getMS(xyzIterator->header.stamp) <= predictdUpToTimestamp + delayXYZ)
00689 xyzIterator++;
00690
00691 std::deque<ardrone_autonomy::Navdata>::iterator rpyIterator = navdataQueue->begin();
00692 while(rpyIterator != navdataQueue->end() &&
00693 getMS(rpyIterator->header.stamp) <= predictdUpToTimestamp + delayRPY)
00694 if(consume)
00695 {
00696 navdataQueue->pop_front();
00697 rpyIterator = navdataQueue->begin();
00698 }
00699 else
00700 rpyIterator++;
00701
00702
00703
00704 while(true)
00705 {
00706
00707 int predictTo = timestamp;
00708
00709
00710 predictTo = min(predictTo, predictdUpToTimestamp+10);
00711
00712
00713
00714 while(xyzIterator != navdataQueue->end() &&
00715 getMS(xyzIterator->header.stamp) - delayXYZ < predictdUpToTimestamp)
00716 xyzIterator++;
00717 while(rpyIterator != navdataQueue->end() &&
00718 getMS(rpyIterator->header.stamp) - delayRPY < predictdUpToTimestamp)
00719 rpyIterator++;
00720
00721 while(controlIterator != velQueue->end() &&
00722 controlIterator+1 != velQueue->end() &&
00723 getMS((controlIterator+1)->header.stamp) + delayControl <= predictdUpToTimestamp)
00724 controlIterator++;
00725
00726
00727
00728
00729 if(rpyIterator != navdataQueue->end() )
00730 predictTo = min(predictTo, getMS(rpyIterator->header.stamp)-delayRPY);
00731 if(xyzIterator != navdataQueue->end() )
00732 predictTo = min(predictTo, getMS(xyzIterator->header.stamp)-delayXYZ);
00733
00734
00735
00736
00737 predictInternal(useControlGains ? controlIterator->twist : geometry_msgs::Twist(),
00738 (predictTo - predictdUpToTimestamp)*1000,
00739 useControlGains &&
00740 getMS(controlIterator->header.stamp) + 200 > predictdUpToTimestamp - delayControl);
00741
00742
00743
00744
00745
00746 bool observedXYZ = false, observedRPY=false;
00747 if(rpyIterator != navdataQueue->end() && getMS(rpyIterator->header.stamp)-delayRPY == predictTo)
00748 {
00749 if(this->useNavdata)
00750 observeIMU_RPY(&(*rpyIterator));
00751
00752 observedRPY = true;
00753
00754 }
00755 if(xyzIterator != navdataQueue->end() && getMS(xyzIterator->header.stamp)-delayXYZ == predictTo)
00756 {
00757 if(this->useNavdata)
00758 observeIMU_XYZ(&(*xyzIterator));
00759
00760 observedXYZ = true;
00761
00762 }
00763
00764
00765 predictdUpToTimestamp = predictTo;
00766
00767 if(consume)
00768 {
00769 if(node->logfileFilter != NULL)
00770 {
00771 pthread_mutex_lock(&(node->logFilter_CS));
00772 (*(node->logfileFilter)) << predictdUpToTimestamp << " " << 0 << " " << 0 << " " << 0 << " " <<
00773 0 << " " << 0 << " " << 0 << " " <<
00774 controlIterator->twist.linear.y << " " << controlIterator->twist.linear.x << " " << controlIterator->twist.linear.z << " " << controlIterator->twist.angular.z << " " <<
00775 (observedRPY ? rpyIterator->rotX : -1) << " " << (observedRPY ? rpyIterator->rotY : -1) << " " << (observedRPY ? lastdYaw : -1) << " " <<
00776 (observedXYZ ? xyzIterator->vx : -1) << " " << (observedXYZ ? xyzIterator->vy : -1) << " " << (observedXYZ ? lastdZ : -1) << " " <<
00777 x.state[0] << " " << y.state[0] << " " << z.state[0] << " " << roll.state << " " << pitch.state << " " << yaw.state[0] << " " << x.state[1] << " " << y.state[1] << " " << z.state[1] << " " << yaw.state[1] << " " <<
00778 lastVXGain << " " << lastVYGain << " " << "\n";
00779 pthread_mutex_unlock(&(node->logFilter_CS));
00780 }
00781 }
00782
00783 if(observedRPY) rpyIterator++;
00784 if(observedXYZ) xyzIterator++;
00785
00786
00787
00788 if(predictTo == timestamp)
00789 break;
00790 }
00791
00792 }
00793
00794
00795 TooN::Vector<3> DroneKalmanFilter::transformPTAMObservation(double x,double y,double z)
00796 {
00797 return transformPTAMObservation(x,y,z,yaw.state[0]);
00798 }
00799 TooN::Vector<3> DroneKalmanFilter::transformPTAMObservation(double x,double y,double z, double yaw)
00800 {
00801 double yawRad = yaw * 3.14159268 / 180;
00802 x = x_offset + xy_scale*x - 0.2*sin(yawRad);
00803 y = y_offset + xy_scale*y - 0.2*cos(yawRad);
00804 z = z_offset + z_scale*z;
00805 return TooN::makeVector(x,y,z);
00806 }
00807
00808 TooN::Vector<6> DroneKalmanFilter::transformPTAMObservation(TooN::Vector<6> obs)
00809 {
00810 obs.slice<0,3>() = transformPTAMObservation(obs[0], obs[1], obs[2], obs[5]);
00811
00812 obs[3] += roll_offset;
00813 obs[4] += pitch_offset;
00814 obs[5] += yaw_offset;
00815
00816 return obs;
00817 }
00818 TooN::Vector<6> DroneKalmanFilter::backTransformPTAMObservation(TooN::Vector<6> obs)
00819 {
00820 obs[3] -= roll_offset;
00821 obs[4] -= pitch_offset;
00822 obs[5] -= yaw_offset;
00823
00824 double yawRad = obs[5] * 3.14159268 / 180;
00825 obs[0] = (- x_offset + obs[0] + 0.2*sin(yawRad))/xy_scale;
00826 obs[1] = (- y_offset + obs[1] + 0.2*cos(yawRad))/xy_scale;
00827 obs[2] = (- z_offset + obs[2])/z_scale;
00828
00829 return obs;
00830 }
00831
00832
00833
00834 TooN::Vector<6> DroneKalmanFilter::getCurrentPose()
00835 {
00836 return TooN::makeVector(x.state[0], y.state[0], z.state[0], roll.state, pitch.state, yaw.state[0]);
00837 }
00838
00839 tum_ardrone::filter_state DroneKalmanFilter::getCurrentPoseSpeed()
00840 {
00841 tum_ardrone::filter_state s;
00842 s.x = x.state[0];
00843 s.y = y.state[0];
00844 s.z = z.state[0];
00845 s.yaw = yaw.state[0];
00846 s.dx = x.state[1];
00847 s.dy = y.state[1];
00848 s.dz = z.state[1];
00849 s.dyaw = yaw.state[1];
00850 s.roll = roll.state;
00851 s.pitch = pitch.state;
00852
00853 if(s.roll*s.roll < 0.001) s.roll = 0;
00854 if(s.pitch*s.pitch < 0.001) s.pitch = 0;
00855 if(s.yaw*s.yaw < 0.001) s.yaw = 0;
00856 if(s.dx*s.dx < 0.001) s.dx = 0;
00857 if(s.dy*s.dy < 0.001) s.dy = 0;
00858 if(s.dz*s.dz < 0.001) s.dz = 0;
00859 if(s.x*s.x < 0.001) s.x = 0;
00860 if(s.y*s.y < 0.001) s.y = 0;
00861 if(s.z*s.z < 0.001) s.z = 0;
00862
00863 return s;
00864 }
00865
00866 TooN::Vector<10> DroneKalmanFilter::getCurrentPoseSpeedAsVec()
00867 {
00868 return TooN::makeVector(x.state[0], y.state[0], z.state[0], roll.state, pitch.state, yaw.state[0],
00869 x.state[1], y.state[1], z.state[1],yaw.state[1]);
00870 }
00871
00872 TooN::Vector<10> DroneKalmanFilter::getCurrentPoseSpeedVariances()
00873 {
00874 return TooN::makeVector(x.var(0,0), y.var(0,0), z.var(0,0), roll.var, pitch.var, yaw.var(0,0),
00875 x.var(1,1), y.var(1,1), z.var(1,1),yaw.var(1,1));
00876 }
00877
00878 TooN::Vector<6> DroneKalmanFilter::getCurrentPoseVariances()
00879 {
00880 return TooN::makeVector(x.var(0,0), y.var(0,0), z.var(0,0), roll.var, pitch.var, yaw.var(0,0));
00881 }
00882 TooN::Vector<6> DroneKalmanFilter::getCurrentOffsets()
00883 {
00884 TooN::Vector<6> res = TooN::makeVector(0,0,0,0,0,0);
00885 if(offsets_xyz_initialized)
00886 res.slice<0,3>() = TooN::makeVector(x_offset, y_offset, z_offset);
00887 if(rp_offset_framesContributed > 1)
00888 res.slice<3,3>() = TooN::makeVector(roll_offset, pitch_offset, yaw_offset);
00889 return res;
00890 }
00891 TooN::Vector<3> DroneKalmanFilter::getCurrentScales()
00892 {
00893 return TooN::makeVector(scale_xyz_initialized ? xy_scale : 1, scale_xyz_initialized ? xy_scale : 1, scale_xyz_initialized ? z_scale : 1);
00894 }
00895 TooN::Vector<3> DroneKalmanFilter::getCurrentScalesForLog()
00896 {
00897 return TooN::makeVector(scale_xyz_initialized ? scale_from_xy : 1, scale_xyz_initialized ? scale_from_z : 1, scale_xyz_initialized ? xy_scale : 1);
00898 }
00899
00900 void DroneKalmanFilter::setCurrentScales(TooN::Vector<3> scales)
00901 {
00902 if(allSyncLocked) return;
00903 xy_scale = scales[0];
00904 z_scale = scales[0];
00905 scale_from_xy = scale_from_z = scales[0];
00906
00907 xyz_sum_IMUxIMU = 0.2 * scales[0];
00908 xyz_sum_PTAMxPTAM = 0.2 / scales[0];
00909 xyz_sum_PTAMxIMU = 0.2;
00910
00911 (*scalePairs).clear();
00912
00913 (*scalePairs).push_back(ScaleStruct(
00914 TooN::makeVector(0.2,0.2,0.2) / sqrt(scales[0]),
00915 TooN::makeVector(0.2,0.2,0.2) * sqrt(scales[0])
00916 ));
00917
00918
00919
00920 scale_xyz_initialized = true;
00921 offsets_xyz_initialized = false;
00922
00923 initialScaleSet = scales[0];
00924 }
00925
00926 void DroneKalmanFilter::addPTAMObservation(TooN::Vector<6> trans, int time)
00927 {
00928 if(time > predictdUpToTimestamp)
00929 predictUpTo(time, true,true);
00930
00931 observePTAM(trans);
00932 numGoodPTAMObservations++;
00933 }
00934 void DroneKalmanFilter::addFakePTAMObservation(int time)
00935 {
00936 if(time > predictdUpToTimestamp)
00937 predictUpTo(time, true,true);
00938
00939 lastPosesValid = false;
00940 }
00941 tum_ardrone::filter_state DroneKalmanFilter::getPoseAt(ros::Time t, bool useControlGains)
00942 {
00943
00944 DroneKalmanFilter scopy = DroneKalmanFilter(*this);
00945
00946
00947 scopy.predictUpTo(getMS(t),false, useControlGains);
00948
00949
00950 return scopy.getCurrentPoseSpeed();
00951 }
00952
00953 TooN::Vector<10> DroneKalmanFilter::getPoseAtAsVec(int timestamp, bool useControlGains)
00954 {
00955
00956 DroneKalmanFilter scopy = DroneKalmanFilter(*this);
00957
00958
00959 scopy.predictUpTo(timestamp,false, useControlGains);
00960
00961
00962 return scopy.getCurrentPoseSpeedAsVec();
00963
00964 }
00965
00966 bool DroneKalmanFilter::handleCommand(std::string s)
00967 {
00968
00969 return false;
00970 }