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