void InertiaSensorFilter::readingUpdate(const Vector3<>& reading) { generateSigmaPoints(); for(int i = 0; i < 5; ++i) readingModel(sigmaPoints[i], sigmaReadings[i]); meanOfSigmaReadings(); // PLOT("module:InertiaSensorFilter:expectedAccX", readingMean.x); // PLOT("module:InertiaSensorFilter:accX", reading.x); // PLOT("module:InertiaSensorFilter:expectedAccY", readingMean.y); // PLOT("module:InertiaSensorFilter:accY", reading.y); // PLOT("module:InertiaSensorFilter:expectedAccZ", readingMean.z); // PLOT("module:InertiaSensorFilter:accZ", reading.z); covOfSigmaReadingsAndSigmaPoints(); covOfSigmaReadings(); const Matrix2x3<float>& kalmanGain = readingsSigmaPointsCov.transpose() * (readingsCov + p.sensorCov).invert(); const Vector2<>& innovation = kalmanGain * (reading - readingMean); x += innovation; cov -= kalmanGain * readingsSigmaPointsCov; }
void InertialDataFilter::readingUpdate(const Vector3f& reading) { generateSigmaPoints(); for(int i = 0; i < 5; ++i) readingModel(sigmaPoints[i], sigmaReadings[i]); meanOfSigmaReadings(); PLOT("module:InertialDataFilter:expectedAccX", readingMean.x()); PLOT("module:InertialDataFilter:accX", reading.x()); PLOT("module:InertialDataFilter:expectedAccY", readingMean.y()); PLOT("module:InertialDataFilter:accY", reading.y()); PLOT("module:InertialDataFilter:expectedAccZ", readingMean.z()); PLOT("module:InertialDataFilter:accZ", reading.z()); const Matrix3x2f readingsSigmaPointsCov = covOfSigmaReadingsAndSigmaPoints(); const Matrix3f readingsCov = covOfSigmaReadings(); const Matrix3f sensorCov = accNoise.array().square().matrix().asDiagonal(); const Matrix2x3f kalmanGain = readingsSigmaPointsCov.transpose() * (readingsCov + sensorCov).inverse(); mean += kalmanGain * (reading - readingMean); cov -= kalmanGain * readingsSigmaPointsCov; }