56 FB_DEBUG(
"---------------------- Ekf::correct ----------------------\n" <<
57 "State is:\n" <<
state_ <<
"\n" 58 "Topic is:\n" << measurement.
topicName_ <<
"\n" 60 "Measurement topic name is:\n" << measurement.
topicName_ <<
"\n\n" 61 "Measurement covariance is:\n" << measurement.
covariance_ <<
"\n");
68 std::vector<size_t> updateIndices;
76 FB_DEBUG(
"Value at index " << i <<
" was nan. Excluding from update.\n");
80 FB_DEBUG(
"Value at index " << i <<
" was inf. Excluding from update.\n");
84 updateIndices.push_back(i);
89 FB_DEBUG(
"Update indices are:\n" << updateIndices <<
"\n");
91 size_t updateSize = updateIndices.size();
94 Eigen::VectorXd stateSubset(updateSize);
95 Eigen::VectorXd measurementSubset(updateSize);
96 Eigen::MatrixXd measurementCovarianceSubset(updateSize, updateSize);
97 Eigen::MatrixXd stateToMeasurementSubset(updateSize,
state_.rows());
98 Eigen::MatrixXd kalmanGainSubset(
state_.rows(), updateSize);
99 Eigen::VectorXd innovationSubset(updateSize);
101 stateSubset.setZero();
102 measurementSubset.setZero();
103 measurementCovarianceSubset.setZero();
104 stateToMeasurementSubset.setZero();
105 kalmanGainSubset.setZero();
106 innovationSubset.setZero();
109 for (
size_t i = 0; i < updateSize; ++i)
111 measurementSubset(i) = measurement.
measurement_(updateIndices[i]);
112 stateSubset(i) =
state_(updateIndices[i]);
114 for (
size_t j = 0; j < updateSize; ++j)
116 measurementCovarianceSubset(i, j) = measurement.
covariance_(updateIndices[i], updateIndices[j]);
122 if (measurementCovarianceSubset(i, i) < 0)
124 FB_DEBUG(
"WARNING: Negative covariance for index " << i <<
125 " of measurement (value is" << measurementCovarianceSubset(i, i) <<
126 "). Using absolute value...\n");
128 measurementCovarianceSubset(i, i) = ::fabs(measurementCovarianceSubset(i, i));
137 if (measurementCovarianceSubset(i, i) < 1e-9)
139 FB_DEBUG(
"WARNING: measurement had very small error covariance for index " << updateIndices[i] <<
140 ". Adding some noise to maintain filter stability.\n");
142 measurementCovarianceSubset(i, i) = 1e-9;
148 for (
size_t i = 0; i < updateSize; ++i)
150 stateToMeasurementSubset(i, updateIndices[i]) = 1;
153 FB_DEBUG(
"Current state subset is:\n" << stateSubset <<
154 "\nMeasurement subset is:\n" << measurementSubset <<
155 "\nMeasurement covariance subset is:\n" << measurementCovarianceSubset <<
156 "\nState-to-measurement subset is:\n" << stateToMeasurementSubset <<
"\n");
160 Eigen::MatrixXd hphrInv = (stateToMeasurementSubset * pht + measurementCovarianceSubset).
inverse();
161 kalmanGainSubset.noalias() = pht * hphrInv;
163 innovationSubset = (measurementSubset - stateSubset);
166 for (
size_t i = 0; i < updateSize; ++i)
172 while (innovationSubset(i) < -
PI)
174 innovationSubset(i) +=
TAU;
177 while (innovationSubset(i) >
PI)
179 innovationSubset(i) -=
TAU;
188 state_.noalias() += kalmanGainSubset * innovationSubset;
191 Eigen::MatrixXd gainResidual =
identity_;
192 gainResidual.noalias() -= kalmanGainSubset * stateToMeasurementSubset;
195 measurementCovarianceSubset *
196 kalmanGainSubset.transpose();
201 FB_DEBUG(
"Kalman gain subset is:\n" << kalmanGainSubset <<
202 "\nInnovation is:\n" << innovationSubset <<
203 "\nCorrected full state is:\n" <<
state_ <<
205 "\n\n---------------------- /Ekf::correct ----------------------\n");
211 FB_DEBUG(
"---------------------- Ekf::predict ----------------------\n" <<
212 "delta is " << delta <<
"\n" <<
213 "state is " <<
state_ <<
"\n");
228 double sp = ::sin(pitch);
229 double cp = ::cos(pitch);
230 double cpi = 1.0 / cp;
231 double tp = sp * cpi;
233 double sr = ::sin(roll);
234 double cr = ::cos(roll);
236 double sy = ::sin(yaw);
237 double cy = ::cos(yaw);
276 double oneHalfATSquared = 0.5 * delta * delta;
278 yCoeff = cy * sp * cr + sy * sr;
279 zCoeff = -cy * sp * sr + sy * cr;
280 double dFx_dR = (yCoeff * yVel + zCoeff * zVel) * delta +
281 (yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
282 double dFR_dR = 1.0 + (cr * tp * pitchVel - sr * tp * yawVel) * delta;
285 yCoeff = cy * cp * sr;
286 zCoeff = cy * cp * cr;
287 double dFx_dP = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
288 (xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
289 double dFR_dP = (cpi * cpi * sr * pitchVel + cpi * cpi * cr * yawVel) * delta;
292 yCoeff = -sy * sp * sr - cy * cr;
293 zCoeff = -sy * sp * cr + cy * sr;
294 double dFx_dY = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
295 (xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
297 yCoeff = sy * sp * cr - cy * sr;
298 zCoeff = -sy * sp * sr - cy * cr;
299 double dFy_dR = (yCoeff * yVel + zCoeff * zVel) * delta +
300 (yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
301 double dFP_dR = (-sr * pitchVel - cr * yawVel) * delta;
304 yCoeff = sy * cp * sr;
305 zCoeff = sy * cp * cr;
306 double dFy_dP = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
307 (xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
310 yCoeff = cy * sp * sr - sy * cr;
311 zCoeff = cy * sp * cr + sy * sr;
312 double dFy_dY = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
313 (xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
317 double dFz_dR = (yCoeff * yVel + zCoeff * zVel) * delta +
318 (yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
319 double dFY_dR = (cr * cpi * pitchVel - sr * cpi * yawVel) * delta;
324 double dFz_dP = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
325 (xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
326 double dFY_dP = (sr * tp * cpi * pitchVel - cr * tp * cpi * yawVel) * delta;
347 "\nCurrent state is:\n" <<
state_ <<
"\n");
375 FB_DEBUG(
"Predicted state is:\n" << state_ <<
385 "\n\n--------------------- /Ekf::predict ----------------------\n");
void correct(const Measurement &measurement)
Carries out the correct step in the predict/update cycle.
Eigen::MatrixXd transferFunctionJacobian_
The Kalman filter transfer function Jacobian.
std::vector< int > controlUpdateVector_
Which control variables are being used (e.g., not every vehicle is controllable in Y or Z) ...
Eigen::MatrixXd estimateErrorCovariance_
This matrix stores the total error in our position estimate (the state_ variable).
Structure used for storing and comparing measurements (for priority queues)
Eigen::MatrixXd transferFunction_
The Kalman filter transfer function.
Eigen::MatrixXd covariance_
Eigen::MatrixXd identity_
We need the identity for a few operations. Better to store it.
void prepareControl(const double referenceTime, const double predictionDelta)
Converts the control term to an acceleration to be applied in the prediction step.
double mahalanobisThresh_
TF2SIMD_FORCE_INLINE Quaternion inverse(const Quaternion &q)
Eigen::MatrixXd dynamicProcessNoiseCovariance_
Gets updated when useDynamicProcessNoise_ is true.
virtual void wrapStateAngles()
Keeps the state Euler angles in the range [-pi, pi].
virtual bool checkMahalanobisThreshold(const Eigen::VectorXd &innovation, const Eigen::MatrixXd &invCovariance, const double nsigmas)
Tests if innovation is within N-sigmas of covariance. Returns true if passed the test.
~Ekf()
Destructor for the Ekf class.
bool useDynamicProcessNoiseCovariance_
If true, uses the robot's vehicle state and the static process noise covariance matrix to generate a ...
Eigen::VectorXd measurement_
void computeDynamicProcessNoiseCovariance(const Eigen::VectorXd &state, const double delta)
Computes a dynamic process noise covariance matrix using the parameterized state. ...
void predict(const double referenceTime, const double delta)
Carries out the predict step in the predict/update cycle.
Eigen::VectorXd controlAcceleration_
Variable that gets updated every time we process a measurement and we have a valid control...
Eigen::VectorXd state_
This is the robot's state vector, which is what we are trying to filter. The values in this vector ar...
Eigen::MatrixXd processNoiseCovariance_
As we move through the world, we follow a predict/update cycle. If one were to imagine a scenario whe...
std::vector< int > updateVector_
const double PI
Common variables.
Ekf(std::vector< double > args=std::vector< double >())
Constructor for the Ekf class.