42 #include <Eigen/Cholesky> 55 assert(args.size() == 3);
57 double alpha = args[0];
58 double kappa = args[1];
59 double beta = args[2];
73 for (
size_t i = 1; i < sigmaCount; ++i)
87 FB_DEBUG(
"---------------------- Ukf::correct ----------------------\n" <<
90 "\nMeasurement covariance is:\n" << measurement.
covariance_ <<
"\n");
108 for (
size_t sigmaInd = 0; sigmaInd <
STATE_SIZE; ++sigmaInd)
119 std::vector<size_t> updateIndices;
120 for (
size_t i = 0; i < measurement.
updateVector_.size(); ++i)
127 FB_DEBUG(
"Value at index " << i <<
" was nan. Excluding from update.\n");
131 FB_DEBUG(
"Value at index " << i <<
" was inf. Excluding from update.\n");
135 updateIndices.push_back(i);
140 FB_DEBUG(
"Update indices are:\n" << updateIndices <<
"\n");
142 size_t updateSize = updateIndices.size();
145 Eigen::VectorXd stateSubset(updateSize);
146 Eigen::VectorXd measurementSubset(updateSize);
147 Eigen::MatrixXd measurementCovarianceSubset(updateSize, updateSize);
148 Eigen::MatrixXd stateToMeasurementSubset(updateSize,
STATE_SIZE);
149 Eigen::MatrixXd kalmanGainSubset(
STATE_SIZE, updateSize);
150 Eigen::VectorXd innovationSubset(updateSize);
151 Eigen::VectorXd predictedMeasurement(updateSize);
152 Eigen::VectorXd sigmaDiff(updateSize);
153 Eigen::MatrixXd predictedMeasCovar(updateSize, updateSize);
154 Eigen::MatrixXd crossCovar(
STATE_SIZE, updateSize);
156 std::vector<Eigen::VectorXd> sigmaPointMeasurements(
sigmaPoints_.size(), Eigen::VectorXd(updateSize));
158 stateSubset.setZero();
159 measurementSubset.setZero();
160 measurementCovarianceSubset.setZero();
161 stateToMeasurementSubset.setZero();
162 kalmanGainSubset.setZero();
163 innovationSubset.setZero();
164 predictedMeasurement.setZero();
165 predictedMeasCovar.setZero();
166 crossCovar.setZero();
169 for (
size_t i = 0; i < updateSize; ++i)
171 measurementSubset(i) = measurement.
measurement_(updateIndices[i]);
172 stateSubset(i) =
state_(updateIndices[i]);
174 for (
size_t j = 0; j < updateSize; ++j)
176 measurementCovarianceSubset(i, j) = measurement.
covariance_(updateIndices[i], updateIndices[j]);
182 if (measurementCovarianceSubset(i, i) < 0)
184 FB_DEBUG(
"WARNING: Negative covariance for index " << i <<
185 " of measurement (value is" << measurementCovarianceSubset(i, i) <<
186 "). Using absolute value...\n");
188 measurementCovarianceSubset(i, i) = ::fabs(measurementCovarianceSubset(i, i));
197 if (measurementCovarianceSubset(i, i) < 1e-9)
199 measurementCovarianceSubset(i, i) = 1e-9;
201 FB_DEBUG(
"WARNING: measurement had very small error covariance for index " <<
203 ". Adding some noise to maintain filter stability.\n");
209 for (
size_t i = 0; i < updateSize; ++i)
211 stateToMeasurementSubset(i, updateIndices[i]) = 1;
214 FB_DEBUG(
"Current state subset is:\n" << stateSubset <<
215 "\nMeasurement subset is:\n" << measurementSubset <<
216 "\nMeasurement covariance subset is:\n" << measurementCovarianceSubset <<
217 "\nState-to-measurement subset is:\n" << stateToMeasurementSubset <<
"\n");
220 for (
size_t sigmaInd = 0; sigmaInd <
sigmaPoints_.size(); ++sigmaInd)
222 sigmaPointMeasurements[sigmaInd] = stateToMeasurementSubset *
sigmaPoints_[sigmaInd];
223 predictedMeasurement.noalias() +=
stateWeights_[sigmaInd] * sigmaPointMeasurements[sigmaInd];
228 for (
size_t sigmaInd = 0; sigmaInd <
sigmaPoints_.size(); ++sigmaInd)
230 sigmaDiff = sigmaPointMeasurements[sigmaInd] - predictedMeasurement;
231 predictedMeasCovar.noalias() +=
covarWeights_[sigmaInd] * (sigmaDiff * sigmaDiff.transpose());
236 Eigen::MatrixXd invInnovCov = (predictedMeasCovar + measurementCovarianceSubset).
inverse();
237 kalmanGainSubset = crossCovar * invInnovCov;
240 innovationSubset = (measurementSubset - predictedMeasurement);
243 for (
size_t i = 0; i < updateSize; ++i)
249 while (innovationSubset(i) < -
PI)
251 innovationSubset(i) +=
TAU;
254 while (innovationSubset(i) >
PI)
256 innovationSubset(i) -=
TAU;
264 state_.noalias() += kalmanGainSubset * innovationSubset;
274 FB_DEBUG(
"Predicated measurement covariance is:\n" << predictedMeasCovar <<
275 "\nCross covariance is:\n" << crossCovar <<
276 "\nKalman gain subset is:\n" << kalmanGainSubset <<
277 "\nInnovation:\n" << innovationSubset <<
278 "\nCorrected full state is:\n" <<
state_ <<
280 "\n\n---------------------- /Ukf::correct ----------------------\n");
286 FB_DEBUG(
"---------------------- Ukf::predict ----------------------\n" <<
287 "delta is " << delta <<
288 "\nstate is " <<
state_ <<
"\n");
295 double sp = ::sin(pitch);
296 double cp = ::cos(pitch);
297 double cpi = 1.0 / cp;
298 double tp = sp * cpi;
300 double sr = ::sin(roll);
301 double cr = ::cos(roll);
303 double sy = ::sin(yaw);
304 double cy = ::cos(yaw);
349 for (
size_t sigmaInd = 0; sigmaInd <
STATE_SIZE; ++sigmaInd)
357 for (
size_t sigmaInd = 0; sigmaInd <
sigmaPoints_.size(); ++sigmaInd)
364 Eigen::VectorXd sigmaDiff(STATE_SIZE);
365 for (
size_t sigmaInd = 0; sigmaInd <
sigmaPoints_.size(); ++sigmaInd)
389 FB_DEBUG(
"Predicted state is:\n" << state_ <<
391 "\n\n--------------------- /Ukf::predict ----------------------\n");
std::vector< Eigen::VectorXd > sigmaPoints_
The UKF sigma points.
Eigen::MatrixXd weightedCovarSqrt_
This matrix is used to generate the sigmaPoints_.
const int STATE_SIZE
Global constants that define our state vector size and offsets to groups of values within that state...
Eigen::MatrixXd estimateErrorCovariance_
This matrix stores the total error in our position estimate (the state_ variable).
void correct(const Measurement &measurement)
Carries out the correct step in the predict/update cycle.
Structure used for storing and comparing measurements (for priority queues)
Eigen::MatrixXd transferFunction_
The Kalman filter transfer function.
Eigen::MatrixXd covariance_
std::vector< double > covarWeights_
The weights associated with each sigma point when calculating a predicted estimateErrorCovariance_.
~Ukf()
Destructor for the Ukf class.
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].
void predict(const double referenceTime, const double delta)
Carries out the predict step in the predict/update cycle.
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.
Ukf(std::vector< double > args)
Constructor for the Ukf class.
bool uncorrected_
Used to determine if we need to re-compute the sigma points when carrying out multiple corrections...
bool useDynamicProcessNoiseCovariance_
If true, uses the robot's vehicle state and the static process noise covariance matrix to generate a ...
std::vector< double > stateWeights_
The weights associated with each sigma point when generating a new state.
Eigen::VectorXd measurement_
void computeDynamicProcessNoiseCovariance(const Eigen::VectorXd &state, const double delta)
Computes a dynamic process noise covariance matrix using the parameterized state. ...
Eigen::VectorXd state_
This is the robot's state vector, which is what we are trying to filter. The values in this vector ar...
double lambda_
Used in weight generation for the sigma points.
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.