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 state_.noalias() += kalmanGainSubset * innovationSubset;
00265
00266
00267 estimateErrorCovariance_.noalias() -= (kalmanGainSubset * predictedMeasCovar * kalmanGainSubset.transpose());
00268
00269 wrapStateAngles();
00270
00271
00272 uncorrected_ = false;
00273
00274 FB_DEBUG("Predicated measurement covariance is:\n" << predictedMeasCovar <<
00275 "\nCross covariance is:\n" << crossCovar <<
00276 "\nKalman gain subset is:\n" << kalmanGainSubset <<
00277 "\nInnovation:\n" << innovationSubset <<
00278 "\nCorrected full state is:\n" << state_ <<
00279 "\nCorrected full estimate error covariance is:\n" << estimateErrorCovariance_ <<
00280 "\n\n---------------------- /Ukf::correct ----------------------\n");
00281 }
00282 }
00283
00284 void Ukf::predict(const double referenceTime, const double delta)
00285 {
00286 FB_DEBUG("---------------------- Ukf::predict ----------------------\n" <<
00287 "delta is " << delta <<
00288 "\nstate is " << state_ << "\n");
00289
00290 double roll = state_(StateMemberRoll);
00291 double pitch = state_(StateMemberPitch);
00292 double yaw = state_(StateMemberYaw);
00293
00294
00295 double sp = ::sin(pitch);
00296 double cp = ::cos(pitch);
00297 double cpi = 1.0 / cp;
00298 double tp = sp * cpi;
00299
00300 double sr = ::sin(roll);
00301 double cr = ::cos(roll);
00302
00303 double sy = ::sin(yaw);
00304 double cy = ::cos(yaw);
00305
00306 prepareControl(referenceTime, delta);
00307
00308
00309 transferFunction_(StateMemberX, StateMemberVx) = cy * cp * delta;
00310 transferFunction_(StateMemberX, StateMemberVy) = (cy * sp * sr - sy * cr) * delta;
00311 transferFunction_(StateMemberX, StateMemberVz) = (cy * sp * cr + sy * sr) * delta;
00312 transferFunction_(StateMemberX, StateMemberAx) = 0.5 * transferFunction_(StateMemberX, StateMemberVx) * delta;
00313 transferFunction_(StateMemberX, StateMemberAy) = 0.5 * transferFunction_(StateMemberX, StateMemberVy) * delta;
00314 transferFunction_(StateMemberX, StateMemberAz) = 0.5 * transferFunction_(StateMemberX, StateMemberVz) * delta;
00315 transferFunction_(StateMemberY, StateMemberVx) = sy * cp * delta;
00316 transferFunction_(StateMemberY, StateMemberVy) = (sy * sp * sr + cy * cr) * delta;
00317 transferFunction_(StateMemberY, StateMemberVz) = (sy * sp * cr - cy * sr) * delta;
00318 transferFunction_(StateMemberY, StateMemberAx) = 0.5 * transferFunction_(StateMemberY, StateMemberVx) * delta;
00319 transferFunction_(StateMemberY, StateMemberAy) = 0.5 * transferFunction_(StateMemberY, StateMemberVy) * delta;
00320 transferFunction_(StateMemberY, StateMemberAz) = 0.5 * transferFunction_(StateMemberY, StateMemberVz) * delta;
00321 transferFunction_(StateMemberZ, StateMemberVx) = -sp * delta;
00322 transferFunction_(StateMemberZ, StateMemberVy) = cp * sr * delta;
00323 transferFunction_(StateMemberZ, StateMemberVz) = cp * cr * delta;
00324 transferFunction_(StateMemberZ, StateMemberAx) = 0.5 * transferFunction_(StateMemberZ, StateMemberVx) * delta;
00325 transferFunction_(StateMemberZ, StateMemberAy) = 0.5 * transferFunction_(StateMemberZ, StateMemberVy) * delta;
00326 transferFunction_(StateMemberZ, StateMemberAz) = 0.5 * transferFunction_(StateMemberZ, StateMemberVz) * delta;
00327 transferFunction_(StateMemberRoll, StateMemberVroll) = delta;
00328 transferFunction_(StateMemberRoll, StateMemberVpitch) = sr * tp * delta;
00329 transferFunction_(StateMemberRoll, StateMemberVyaw) = cr * tp * delta;
00330 transferFunction_(StateMemberPitch, StateMemberVpitch) = cr * delta;
00331 transferFunction_(StateMemberPitch, StateMemberVyaw) = -sr * delta;
00332 transferFunction_(StateMemberYaw, StateMemberVpitch) = sr * cpi * delta;
00333 transferFunction_(StateMemberYaw, StateMemberVyaw) = cr * cpi * delta;
00334 transferFunction_(StateMemberVx, StateMemberAx) = delta;
00335 transferFunction_(StateMemberVy, StateMemberAy) = delta;
00336 transferFunction_(StateMemberVz, StateMemberAz) = delta;
00337
00338
00339 weightedCovarSqrt_ = ((STATE_SIZE + lambda_) * estimateErrorCovariance_).llt().matrixL();
00340
00341
00342
00343
00344
00345 sigmaPoints_[0] = transferFunction_ * state_;
00346
00347
00348
00349 for (size_t sigmaInd = 0; sigmaInd < STATE_SIZE; ++sigmaInd)
00350 {
00351 sigmaPoints_[sigmaInd + 1] = transferFunction_ * (state_ + weightedCovarSqrt_.col(sigmaInd));
00352 sigmaPoints_[sigmaInd + 1 + STATE_SIZE] = transferFunction_ * (state_ - weightedCovarSqrt_.col(sigmaInd));
00353 }
00354
00355
00356 state_.setZero();
00357 for (size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd)
00358 {
00359 state_.noalias() += stateWeights_[sigmaInd] * sigmaPoints_[sigmaInd];
00360 }
00361
00362
00363 estimateErrorCovariance_.setZero();
00364 Eigen::VectorXd sigmaDiff(STATE_SIZE);
00365 for (size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd)
00366 {
00367 sigmaDiff = (sigmaPoints_[sigmaInd] - state_);
00368 estimateErrorCovariance_.noalias() += covarWeights_[sigmaInd] * (sigmaDiff * sigmaDiff.transpose());
00369 }
00370
00371
00372
00373 Eigen::MatrixXd *processNoiseCovariance = &processNoiseCovariance_;
00374
00375 if (useDynamicProcessNoiseCovariance_)
00376 {
00377 computeDynamicProcessNoiseCovariance(state_, delta);
00378 processNoiseCovariance = &dynamicProcessNoiseCovariance_;
00379 }
00380
00381 estimateErrorCovariance_.noalias() += delta * (*processNoiseCovariance);
00382
00383
00384 wrapStateAngles();
00385
00386
00387 uncorrected_ = true;
00388
00389 FB_DEBUG("Predicted state is:\n" << state_ <<
00390 "\nPredicted estimate error covariance is:\n" << estimateErrorCovariance_ <<
00391 "\n\n--------------------- /Ukf::predict ----------------------\n");
00392 }
00393
00394 }