00001 #pragma once
00002
00022 #ifndef __DRONEKALMANFILTER_H
00023 #define __DRONEKALMANFILTER_H
00024
00025 #include <TooN/TooN.h>
00026 #include <deque>
00027 #include <iostream>
00028 #include <fstream>
00029 #include <ardrone_autonomy/Navdata.h>
00030 #include <geometry_msgs/Twist.h>
00031 #include <geometry_msgs/TwistStamped.h>
00032 #include <pthread.h>
00033 #include "../HelperFunctions.h"
00034 #include "uga_tum_ardrone/filter_state.h"
00035
00036 class EstimationNode;
00037
00038
00039 class ScaleStruct
00040 {
00041 public:
00042 TooN::Vector<3> ptam;
00043 TooN::Vector<3> imu;
00044 double ptamNorm;
00045 double imuNorm;
00046 double alphaSingleEstimate;
00047 double pp, ii, pi;
00048
00049 inline double computeEstimator(double spp, double sii, double spi, double stdDevPTAM = 0.2, double stdDevIMU = 0.1)
00050 {
00051 double sII = stdDevPTAM * stdDevPTAM * sii;
00052 double sPP = stdDevIMU * stdDevIMU * spp;
00053 double sPI = stdDevIMU * stdDevPTAM * spi;
00054
00055 double tmp = (sII-sPP)*(sII-sPP) + 4*sPI*sPI;
00056 if(tmp <= 0) tmp = 1e-5;
00057 return 0.5*((sII-sPP)+sqrt(tmp)) / (stdDevPTAM * stdDevPTAM * spi);
00058
00059 }
00060
00061 inline ScaleStruct(TooN::Vector<3> ptamDist, TooN::Vector<3> imuDist)
00062 {
00063 ptam = ptamDist;
00064 imu = imuDist;
00065 pp = ptam[0]*ptam[0] + ptam[1]*ptam[1] + ptam[2]*ptam[2];
00066 ii = imu[0]*imu[0] + imu[1]*imu[1] + imu[2]*imu[2];
00067 pi = imu[0]*ptam[0] + imu[1]*ptam[1] + imu[2]*ptam[2];
00068
00069 ptamNorm = sqrt(pp);
00070 imuNorm = sqrt(ii);
00071
00072 alphaSingleEstimate = computeEstimator(pp,ii,pi);
00073 }
00074
00075 inline bool operator < (const ScaleStruct& comp) const
00076 {
00077 return alphaSingleEstimate < comp.alphaSingleEstimate;
00078 }
00079 };
00080
00081
00082
00083 class PVFilter
00084 {
00085 public:
00086 TooN::Vector<2> state;
00087 TooN::Matrix<2,2> var;
00088
00089
00090
00091 inline PVFilter(TooN::Vector<2> state, TooN::Matrix<2,2> var)
00092 : state(state), var(var)
00093 {
00094 }
00095
00096 inline PVFilter(double pose, double speed)
00097 : state(TooN::makeVector(pose,speed)), var(TooN::Zeros)
00098 {
00099 }
00100
00101 inline PVFilter(double pose)
00102 : state(TooN::makeVector(pose,0)), var(TooN::Zeros)
00103 {
00104 var(1,1) = 1e10;
00105 }
00106
00107 inline PVFilter()
00108 : state(TooN::makeVector(0,0)), var(TooN::Zeros)
00109 {
00110 var(1,1) = var(0,0) = 1e10;
00111 }
00112
00113
00114 inline void observePose(double obs, double obsVar)
00115 {
00116
00117
00118
00119
00120
00121
00122 TooN::Vector<2> K = var.T()[0] / (obsVar + var(0,0));
00123 state = state + K * (obs - state[0]);
00124 TooN::Matrix<2> tmp = TooN::Identity;
00125 tmp(0,0) -= K[0];
00126 tmp(1,0) -= K[1];
00127 var = tmp * var;
00128
00129 }
00130
00131
00132 inline void observeSpeed(double obs, double obsVar)
00133 {
00134
00135
00136
00137
00138
00139
00140 TooN::Vector<2> K = var.T()[1] / (obsVar + var(1,1));
00141 state = state + K * (obs - state[1]);
00142 TooN::Matrix<2> tmp = TooN::Identity;
00143 tmp(0,1) -= K[0];
00144 tmp(1,1) -= K[1];
00145 var = tmp * var;
00146 }
00147
00148
00149
00150
00151 inline void predict(double ms, double accelerationVar, TooN::Vector<2> controlGains = TooN::makeVector(0,0), double coVarFac = 1, double speedVarFac = 1)
00152 {
00153
00154
00155
00156
00157
00158
00159
00160 ms /= 1000;
00161
00162 TooN::Matrix<2,2> G = TooN::Identity;
00163 G(0,1) = ms;
00164
00165 state = G * state + controlGains;
00166 var = G * var * G.T();
00167 var(0,0) += accelerationVar * 0.25 * ms*ms*ms*ms;
00168 var(1,0) += coVarFac * accelerationVar * 0.5 * ms*ms*ms;
00169 var(0,1) += coVarFac * accelerationVar * 0.5 * ms*ms*ms;
00170 var(1,1) += speedVarFac * accelerationVar * 1 * ms*ms;
00171 }
00172
00173
00174
00175
00176 inline void predict(double ms, TooN::Vector<3> vars, TooN::Vector<2> controlGains = TooN::makeVector(0,0))
00177 {
00178
00179
00180
00181
00182
00183
00184
00185 ms /= 1000;
00186
00187 TooN::Matrix<2,2> G = TooN::Identity;
00188 G(0,1) = ms;
00189
00190 state = G * state + controlGains;
00191 var = G * var * G.T();
00192 var(0,0) += vars[0];
00193 var(1,0) += vars[2];
00194 var(0,1) += vars[2];
00195 var(1,1) += vars[1];
00196 }
00197 };
00198
00199
00200
00201 class PFilter
00202 {
00203 public:
00204 double state;
00205 double var;
00206
00207 inline PFilter() : state(0), var(1e10) {};
00208 inline PFilter(double initState) : state(initState), var(0) {};
00209
00210 inline void predict(double ms, double speedVar, double controlGains = 0)
00211 {
00212
00213
00214
00215
00216 state += controlGains;
00217 var += speedVar * ms * ms / 1000000;
00218 }
00219
00220 inline void observe(double obs, double obsVar)
00221 {
00222
00223
00224
00225
00226
00227 double w = var / (var + obsVar);
00228 state = (1-w) * state + w * obs;
00229 var = var * obsVar / (var + obsVar);
00230 }
00231 };
00232
00233
00234 class DroneKalmanFilter
00235 {
00236 private:
00237
00238 PVFilter x;
00239 PVFilter y;
00240 PVFilter z;
00241 PFilter roll;
00242 PFilter pitch;
00243 PVFilter yaw;
00244
00245
00246
00247 double x_offset, y_offset, z_offset;
00248 double xy_scale, z_scale;
00249 double scale_from_z;
00250 double scale_from_xy;
00251 double roll_offset, pitch_offset, yaw_offset;
00252 bool offsets_xyz_initialized;
00253 bool scale_xyz_initialized;
00254
00255
00256 double xyz_sum_IMUxIMU;
00257 double xyz_sum_PTAMxPTAM;
00258 double xyz_sum_PTAMxIMU;
00259 double rp_offset_framesContributed;
00260 std::vector<ScaleStruct>* scalePairs;
00261
00262
00263 double last_z_IMU;
00264 long last_yaw_droneTime;
00265 long last_z_droneTime;
00266 long last_z_packageID;
00267
00268
00269 TooN::Vector<3> last_PTAM_pose;
00270
00271
00272 TooN::Vector<3> last_fused_pose;
00273 bool lastPosesValid;
00274
00275
00276
00277 int numGoodIMUObservations;
00278 int numGoodPTAMObservations;
00279
00280
00281 long lastIMU_XYZ_dronetime;
00282 long lastIMU_RPY_dronetime;
00283 long lastIMU_dronetime;
00284 int lastIMU_XYZ_ID;
00285 int lastIMU_RPY_ID;
00286 double lastPredictedRoll;
00287 double lastPredictedPitch;
00288 double initialScaleSet;
00289
00290
00291 void predictInternal(geometry_msgs::Twist activeControlInfo, int timeSpanMicros, bool useControlGains = true);
00292 void observeIMU_XYZ(const ardrone_autonomy::Navdata* nav);
00293 void observeIMU_RPY(const ardrone_autonomy::Navdata* nav);
00294 void observePTAM(TooN::Vector<6> pose);
00295
00296
00297
00298 void sync_xyz(double x_global, double y_global, double z_global);
00299 void sync_rpy(double roll_global, double pitch_global, double yaw_global);
00300
00301
00302 int predictedUpToTotal;
00303 long predictedUpToDroneTime;
00304
00305
00306 double lastdYaw, lastdZ;
00307 double baselineZ_IMU;
00308 double baselineZ_Filter;
00309 double last_z_heightDiff;
00310 double baselineY_IMU;
00311 bool baselinesYValid;
00312 int timestampYawBaselineFrom;
00313 double lastVXGain;
00314 double lastVYGain;
00315
00316
00317
00318 EstimationNode* node;
00319 public:
00320 DroneKalmanFilter(EstimationNode* node);
00321 ~DroneKalmanFilter(void);
00322 void release();
00323
00324 static int delayRPY;
00325 static int delayXYZ;
00326 static int delayVideo;
00327 static int delayControl;
00328
00329 static const int base_delayXYZ;
00330 static const int base_delayVideo;
00331 static const int base_delayControl;
00332
00333 std::deque<ardrone_autonomy::Navdata>* navdataQueue;
00334 std::deque<geometry_msgs::TwistStamped>* velQueue;
00335 static pthread_mutex_t filter_CS;
00336
00337
00338 int predictdUpToTimestamp;
00339 int scalePairsIn, scalePairsOut;
00340
00341
00342
00343
00344 void reset();
00345
00346
00347
00348 void clearPTAM();
00349
00350
00351
00352 void predictUpTo(int timestamp, bool consume = true, bool useControlGains = true);
00353
00354 void setPing(unsigned int navPing, unsigned int vidPing);
00355
00356
00357 TooN::Vector<6> getCurrentPose();
00358 uga_tum_ardrone::filter_state getCurrentPoseSpeed();
00359 TooN::Vector<10> getCurrentPoseSpeedAsVec();
00360 TooN::Vector<10> getCurrentPoseSpeedVariances();
00361 TooN::Vector<6> getCurrentPoseVariances();
00362 TooN::Vector<6> getCurrentOffsets();
00363 TooN::Vector<3> getCurrentScales();
00364 TooN::Vector<3> getCurrentScalesForLog();
00365 float getScaleAccuracy();
00366 void setCurrentScales(TooN::Vector<3> scales);
00367
00368
00369 void updateScaleXYZ(TooN::Vector<3> ptamDiff, TooN::Vector<3> imuDiff, TooN::Vector<3> OrgPtamPose);
00370
00371
00372
00373
00374
00375
00376
00377
00378 TooN::Vector<3> transformPTAMObservation(double x,double y,double z);
00379 TooN::Vector<3> transformPTAMObservation(double x,double y,double z, double yaw);
00380 TooN::Vector<6> transformPTAMObservation(TooN::Vector<6> obs);
00381 TooN::Vector<6> backTransformPTAMObservation(TooN::Vector<6> obs);
00382
00383 inline int getNumGoodPTAMObservations() {return numGoodPTAMObservations;}
00384
00385
00386
00387 TooN::Vector<3> scalingFixpoint;
00388 bool useScalingFixpoint;
00389
00390
00391 void flushScalePairs();
00392
00393
00394 bool allSyncLocked;
00395 bool useControl;
00396 bool useNavdata;
00397 bool usePTAM;
00398
00399
00400 float c1;
00401 float c2;
00402 float c3;
00403 float c4;
00404 float c5;
00405 float c6;
00406 float c7;
00407 float c8;
00408
00409
00410 float yawDriftThreshold;
00411 float zDriftThreshold;
00412
00413
00414 bool handleCommand(std::string s);
00415
00416
00417 void addPTAMObservation(TooN::Vector<6> trans, int time);
00418 void addFakePTAMObservation(int time);
00419 uga_tum_ardrone::filter_state getPoseAt(ros::Time t, bool useControlGains = true);
00420 TooN::Vector<10> getPoseAtAsVec(int timestamp, bool useControlGains = true);
00421
00422 };
00423 #endif