diff --git a/Fusion/FusionCalibration.h b/Fusion/FusionCalibration.h
index 9193e04..733f572 100644
--- a/Fusion/FusionCalibration.h
+++ b/Fusion/FusionCalibration.h
@@ -35,7 +35,7 @@ static inline FusionVector FusionCalibrationInertial(const FusionVector uncalibr
* @return Calibrated measurement.
*/
static inline FusionVector FusionCalibrationMagnetic(const FusionVector uncalibrated, const FusionMatrix softIronMatrix, const FusionVector hardIronOffset) {
- return FusionVectorSubtract(FusionMatrixMultiplyVector(softIronMatrix, uncalibrated), hardIronOffset);
+ return FusionMatrixMultiplyVector(softIronMatrix, FusionVectorSubtract(uncalibrated, hardIronOffset));
}
#endif
diff --git a/README.md b/README.md
index b9e85d0..2e6a655 100644
--- a/README.md
+++ b/README.md
@@ -101,7 +101,7 @@ ic = Ms(iu - b)
The `FusionCalibrationMagnetic` function applies magnetometer calibration parameters using the calibration model:
-mc = Smu - h
+mc = S(mu - h)
- mc is the calibrated magnetometer measurement and `return` value
- mu is the uncalibrated magnetometer measurement and `uncalibrated` argument