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
00033 #include "robot_localization/ukf.h"
00034 #include "robot_localization/filter_common.h"
00035
00036 #include <XmlRpcException.h>
00037
00038 #include <sstream>
00039 #include <iomanip>
00040 #include <limits>
00041
00042 #include <Eigen/Cholesky>
00043
00044 #include <iostream>
00045 #include <vector>
00046
00047 #include <assert.h>
00048
00049 namespace RobotLocalization
00050 {
00051 Ukf::Ukf(std::vector<double> args) :
00052 FilterBase(),
00053 uncorrected_(true)
00054 {
00055 assert(args.size() == 3);
00056
00057 double alpha = args[0];
00058 double kappa = args[1];
00059 double beta = args[2];
00060
00061 size_t sigmaCount = (STATE_SIZE << 1) + 1;
00062 sigmaPoints_.resize(sigmaCount, Eigen::VectorXd(STATE_SIZE));
00063
00064
00065 lambda_ = alpha * alpha * (STATE_SIZE + kappa) - STATE_SIZE;
00066
00067 stateWeights_.resize(sigmaCount);
00068 covarWeights_.resize(sigmaCount);
00069
00070 stateWeights_[0] = lambda_ / (STATE_SIZE + lambda_);
00071 covarWeights_[0] = stateWeights_[0] + (1 - (alpha * alpha) + beta);
00072 sigmaPoints_[0].setZero();
00073 for (size_t i = 1; i < sigmaCount; ++i)
00074 {
00075 sigmaPoints_[i].setZero();
00076 stateWeights_[i] = 1 / (2 * (STATE_SIZE + lambda_));
00077 covarWeights_[i] = stateWeights_[i];
00078 }
00079 }
00080
00081 Ukf::~Ukf()
00082 {
00083 }
00084
00085 void Ukf::correct(const Measurement &measurement)
00086 {
00087 FB_DEBUG("---------------------- Ukf::correct ----------------------\n" <<
00088 "State is:\n" << state_ <<
00089 "\nMeasurement is:\n" << measurement.measurement_ <<
00090 "\nMeasurement covariance is:\n" << measurement.covariance_ << "\n");
00091
00092
00093
00094
00095
00096 if (!uncorrected_)
00097 {
00098
00099 weightedCovarSqrt_ = ((STATE_SIZE + lambda_) * estimateErrorCovariance_).llt().matrixL();
00100
00101
00102
00103
00104 sigmaPoints_[0] = state_;
00105
00106
00107
00108 for (size_t sigmaInd = 0; sigmaInd < STATE_SIZE; ++sigmaInd)
00109 {
00110 sigmaPoints_[sigmaInd + 1] = state_ + weightedCovarSqrt_.col(sigmaInd);
00111 sigmaPoints_[sigmaInd + 1 + STATE_SIZE] = state_ - weightedCovarSqrt_.col(sigmaInd);
00112 }
00113 }
00114
00115
00116
00117
00118
00119 std::vector<size_t> updateIndices;
00120 for (size_t i = 0; i < measurement.updateVector_.size(); ++i)
00121 {
00122 if (measurement.updateVector_[i])
00123 {
00124
00125 if (std::isnan(measurement.measurement_(i)))
00126 {
00127 FB_DEBUG("Value at index " << i << " was nan. Excluding from update.\n");
00128 }
00129 else if (std::isinf(measurement.measurement_(i)))
00130 {
00131 FB_DEBUG("Value at index " << i << " was inf. Excluding from update.\n");
00132 }
00133 else
00134 {
00135 updateIndices.push_back(i);
00136 }
00137 }
00138 }
00139
00140 FB_DEBUG("Update indices are:\n" << updateIndices << "\n");
00141
00142 size_t updateSize = updateIndices.size();
00143
00144
00145 Eigen::VectorXd stateSubset(updateSize);
00146 Eigen::VectorXd measurementSubset(updateSize);
00147 Eigen::MatrixXd measurementCovarianceSubset(updateSize, updateSize);
00148 Eigen::MatrixXd stateToMeasurementSubset(updateSize, STATE_SIZE);
00149 Eigen::MatrixXd kalmanGainSubset(STATE_SIZE, updateSize);
00150 Eigen::VectorXd innovationSubset(updateSize);
00151 Eigen::VectorXd predictedMeasurement(updateSize);
00152 Eigen::VectorXd sigmaDiff(updateSize);
00153 Eigen::MatrixXd predictedMeasCovar(updateSize, updateSize);
00154 Eigen::MatrixXd crossCovar(STATE_SIZE, updateSize);
00155
00156 std::vector<Eigen::VectorXd> sigmaPointMeasurements(sigmaPoints_.size(), Eigen::VectorXd(updateSize));
00157
00158 stateSubset.setZero();
00159 measurementSubset.setZero();
00160 measurementCovarianceSubset.setZero();
00161 stateToMeasurementSubset.setZero();
00162 kalmanGainSubset.setZero();
00163 innovationSubset.setZero();
00164 predictedMeasurement.setZero();
00165 predictedMeasCovar.setZero();
00166 crossCovar.setZero();
00167
00168
00169 for (size_t i = 0; i < updateSize; ++i)
00170 {
00171 measurementSubset(i) = measurement.measurement_(updateIndices[i]);
00172 stateSubset(i) = state_(updateIndices[i]);
00173
00174 for (size_t j = 0; j < updateSize; ++j)
00175 {
00176 measurementCovarianceSubset(i, j) = measurement.covariance_(updateIndices[i], updateIndices[j]);
00177 }
00178
00179
00180
00181
00182 if (measurementCovarianceSubset(i, i) < 0)
00183 {
00184 FB_DEBUG("WARNING: Negative covariance for index " << i <<
00185 " of measurement (value is" << measurementCovarianceSubset(i, i) <<
00186 "). Using absolute value...\n");
00187
00188 measurementCovarianceSubset(i, i) = ::fabs(measurementCovarianceSubset(i, i));
00189 }
00190
00191
00192
00193
00194
00195
00196
00197 if (measurementCovarianceSubset(i, i) < 1e-9)
00198 {
00199 measurementCovarianceSubset(i, i) = 1e-9;
00200
00201 FB_DEBUG("WARNING: measurement had very small error covariance for index " <<
00202 updateIndices[i] <<
00203 ". Adding some noise to maintain filter stability.\n");
00204 }
00205 }
00206
00207
00208
00209 for (size_t i = 0; i < updateSize; ++i)
00210 {
00211 stateToMeasurementSubset(i, updateIndices[i]) = 1;
00212 }
00213
00214 FB_DEBUG("Current state subset is:\n" << stateSubset <<
00215 "\nMeasurement subset is:\n" << measurementSubset <<
00216 "\nMeasurement covariance subset is:\n" << measurementCovarianceSubset <<
00217 "\nState-to-measurement subset is:\n" << stateToMeasurementSubset << "\n");
00218
00219
00220 for (size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd)
00221 {
00222 sigmaPointMeasurements[sigmaInd] = stateToMeasurementSubset * sigmaPoints_[sigmaInd];
00223 predictedMeasurement.noalias() += stateWeights_[sigmaInd] * sigmaPointMeasurements[sigmaInd];
00224 }
00225
00226
00227
00228 for (size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd)
00229 {
00230 sigmaDiff = sigmaPointMeasurements[sigmaInd] - predictedMeasurement;
00231 predictedMeasCovar.noalias() += covarWeights_[sigmaInd] * (sigmaDiff * sigmaDiff.transpose());
00232 crossCovar.noalias() += covarWeights_[sigmaInd] * ((sigmaPoints_[sigmaInd] - state_) * sigmaDiff.transpose());
00233 }
00234
00235
00236 Eigen::MatrixXd invInnovCov = (predictedMeasCovar + measurementCovarianceSubset).inverse();
00237 kalmanGainSubset = crossCovar * invInnovCov;
00238
00239
00240 innovationSubset = (measurementSubset - predictedMeasurement);
00241
00242
00243 for (size_t i = 0; i < updateSize; ++i)
00244 {
00245 if (updateIndices[i] == StateMemberRoll ||
00246 updateIndices[i] == StateMemberPitch ||
00247 updateIndices[i] == StateMemberYaw)
00248 {
00249 while (innovationSubset(i) < -PI)
00250 {
00251 innovationSubset(i) += TAU;
00252 }
00253
00254 while (innovationSubset(i) > PI)
00255 {
00256 innovationSubset(i) -= TAU;
00257 }
00258 }
00259 }
00260
00261
00262 if (checkMahalanobisThreshold(innovationSubset, invInnovCov, measurement.mahalanobisThresh_))
00263 {
00264
00265 state_.noalias() += kalmanGainSubset * innovationSubset;
00266
00267
00268 estimateErrorCovariance_.noalias() -= (kalmanGainSubset * predictedMeasCovar * kalmanGainSubset.transpose());
00269
00270 wrapStateAngles();
00271
00272
00273 uncorrected_ = false;
00274
00275 FB_DEBUG("Predicated measurement covariance is:\n" << predictedMeasCovar <<
00276 "\nCross covariance is:\n" << crossCovar <<
00277 "\nKalman gain subset is:\n" << kalmanGainSubset <<
00278 "\nInnovation:\n" << innovationSubset <<
00279 "\nCorrected full state is:\n" << state_ <<
00280 "\nCorrected full estimate error covariance is:\n" << estimateErrorCovariance_ <<
00281 "\n\n---------------------- /Ukf::correct ----------------------\n");
00282 }
00283 }
00284
00285 void Ukf::predict(const double referenceTime, const double delta)
00286 {
00287 FB_DEBUG("---------------------- Ukf::predict ----------------------\n" <<
00288 "delta is " << delta <<
00289 "\nstate is " << state_ << "\n");
00290
00291 double roll = state_(StateMemberRoll);
00292 double pitch = state_(StateMemberPitch);
00293 double yaw = state_(StateMemberYaw);
00294
00295
00296 double sp = ::sin(pitch);
00297 double cp = ::cos(pitch);
00298
00299 double sr = ::sin(roll);
00300 double cr = ::cos(roll);
00301
00302 double sy = ::sin(yaw);
00303 double cy = ::cos(yaw);
00304
00305 prepareControl(referenceTime, delta);
00306
00307
00308 transferFunction_(StateMemberX, StateMemberVx) = cy * cp * delta;
00309 transferFunction_(StateMemberX, StateMemberVy) = (cy * sp * sr - sy * cr) * delta;
00310 transferFunction_(StateMemberX, StateMemberVz) = (cy * sp * cr + sy * sr) * delta;
00311 transferFunction_(StateMemberX, StateMemberAx) = 0.5 * transferFunction_(StateMemberX, StateMemberVx) * delta;
00312 transferFunction_(StateMemberX, StateMemberAy) = 0.5 * transferFunction_(StateMemberX, StateMemberVy) * delta;
00313 transferFunction_(StateMemberX, StateMemberAz) = 0.5 * transferFunction_(StateMemberX, StateMemberVz) * delta;
00314 transferFunction_(StateMemberY, StateMemberVx) = sy * cp * delta;
00315 transferFunction_(StateMemberY, StateMemberVy) = (sy * sp * sr + cy * cr) * delta;
00316 transferFunction_(StateMemberY, StateMemberVz) = (sy * sp * cr - cy * sr) * delta;
00317 transferFunction_(StateMemberY, StateMemberAx) = 0.5 * transferFunction_(StateMemberY, StateMemberVx) * delta;
00318 transferFunction_(StateMemberY, StateMemberAy) = 0.5 * transferFunction_(StateMemberY, StateMemberVy) * delta;
00319 transferFunction_(StateMemberY, StateMemberAz) = 0.5 * transferFunction_(StateMemberY, StateMemberVz) * delta;
00320 transferFunction_(StateMemberZ, StateMemberVx) = -sp * delta;
00321 transferFunction_(StateMemberZ, StateMemberVy) = cp * sr * delta;
00322 transferFunction_(StateMemberZ, StateMemberVz) = cp * cr * delta;
00323 transferFunction_(StateMemberZ, StateMemberAx) = 0.5 * transferFunction_(StateMemberZ, StateMemberVx) * delta;
00324 transferFunction_(StateMemberZ, StateMemberAy) = 0.5 * transferFunction_(StateMemberZ, StateMemberVy) * delta;
00325 transferFunction_(StateMemberZ, StateMemberAz) = 0.5 * transferFunction_(StateMemberZ, StateMemberVz) * delta;
00326 transferFunction_(StateMemberRoll, StateMemberVroll) = transferFunction_(StateMemberX, StateMemberVx);
00327 transferFunction_(StateMemberRoll, StateMemberVpitch) = transferFunction_(StateMemberX, StateMemberVy);
00328 transferFunction_(StateMemberRoll, StateMemberVyaw) = transferFunction_(StateMemberX, StateMemberVz);
00329 transferFunction_(StateMemberPitch, StateMemberVroll) = transferFunction_(StateMemberY, StateMemberVx);
00330 transferFunction_(StateMemberPitch, StateMemberVpitch) = transferFunction_(StateMemberY, StateMemberVy);
00331 transferFunction_(StateMemberPitch, StateMemberVyaw) = transferFunction_(StateMemberY, StateMemberVz);
00332 transferFunction_(StateMemberYaw, StateMemberVroll) = transferFunction_(StateMemberZ, StateMemberVx);
00333 transferFunction_(StateMemberYaw, StateMemberVpitch) = transferFunction_(StateMemberZ, StateMemberVy);
00334 transferFunction_(StateMemberYaw, StateMemberVyaw) = transferFunction_(StateMemberZ, StateMemberVz);
00335 transferFunction_(StateMemberVx, StateMemberAx) = delta;
00336 transferFunction_(StateMemberVy, StateMemberAy) = delta;
00337 transferFunction_(StateMemberVz, StateMemberAz) = delta;
00338
00339
00340 weightedCovarSqrt_ = ((STATE_SIZE + lambda_) * estimateErrorCovariance_).llt().matrixL();
00341
00342
00343
00344
00345
00346 sigmaPoints_[0] = transferFunction_ * state_;
00347
00348
00349
00350 for (size_t sigmaInd = 0; sigmaInd < STATE_SIZE; ++sigmaInd)
00351 {
00352 sigmaPoints_[sigmaInd + 1] = transferFunction_ * (state_ + weightedCovarSqrt_.col(sigmaInd));
00353 sigmaPoints_[sigmaInd + 1 + STATE_SIZE] = transferFunction_ * (state_ - weightedCovarSqrt_.col(sigmaInd));
00354 }
00355
00356
00357 state_.setZero();
00358 for (size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd)
00359 {
00360 state_.noalias() += stateWeights_[sigmaInd] * sigmaPoints_[sigmaInd];
00361 }
00362
00363
00364 estimateErrorCovariance_.setZero();
00365 Eigen::VectorXd sigmaDiff(STATE_SIZE);
00366 for (size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd)
00367 {
00368 sigmaDiff = (sigmaPoints_[sigmaInd] - state_);
00369 estimateErrorCovariance_.noalias() += covarWeights_[sigmaInd] * (sigmaDiff * sigmaDiff.transpose());
00370 }
00371
00372
00373
00374 Eigen::MatrixXd *processNoiseCovariance = &processNoiseCovariance_;
00375
00376 if (useDynamicProcessNoiseCovariance_)
00377 {
00378 computeDynamicProcessNoiseCovariance(state_, delta);
00379 processNoiseCovariance = &dynamicProcessNoiseCovariance_;
00380 }
00381
00382 estimateErrorCovariance_.noalias() += delta * (*processNoiseCovariance);
00383
00384
00385 wrapStateAngles();
00386
00387
00388 uncorrected_ = true;
00389
00390 FB_DEBUG("Predicted state is:\n" << state_ <<
00391 "\nPredicted estimate error covariance is:\n" << estimateErrorCovariance_ <<
00392 "\n\n--------------------- /Ukf::predict ----------------------\n");
00393 }
00394
00395 }