# Refactoring of Madgwick filter update

The Madgwick filter implementation in `MadgwickFilter::updateOrientation(const xyz_t& gyroRPS, const xyz_t& accelerometer, float deltaT)` has been refactored to
improve computational efficiency.

The steps taken in that refactoring are outlined here.

A similar refactoring was done for `MadgwickFilter::updateOrientation(const xyz_t& gyroRPS, const xyz_t& accelerometer, xyz_t& magnetometer, float deltaT)`.

## Derivation

The original code as used in many implementations (Arduino, Adafruit, M5Stack, Reefwing-AHRS):

```cpp
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0 * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0 * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0 * q1q1 * q3 - _2q1 * ax + 4.0 * q2q2 * q3 - _2q2 * ay;
```

Reorder terms:

```cpp
s0 = _4q0 * q2q2 + _4q0 * q1q1                                                     + _2q2 * ax - _2q1 * ay;
s1 = _4q1 * q3q3 + 4.0 * q0q0 * q1 - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az - _2q3 * ax - _2q0 * ay;
s2 = 4.0 * q0q0 * q2 + _4q2 * q3q3 - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az + _2q0 * ax - _2q3 * ay;
s3 = 4.0 * q1q1 * q3  + 4.0 * q2q2 * q3                                          - _2q1 * ax - _2q2 * ay;
```

Factor out `4 * q<n>`:

```cpp
s0 = 4 * q0 * (q2q2 + q1q1)                            + _2q2 * ax - _2q1 * ay;
s1 = 4 * q1 * (q3q3 + q0q0 - 1 + 2*q1q1 + 2*q2q2 + az) - _2q3 * ax - _2q0 * ay;
s2 = 4 * q2 * (q0q0 + q3q3 - 1 + 2*q1q1 + 2*q2q2 + az) + _2q0 * ax - _2q3 * ay;
s3 = 4 * q3 * (q1q1 + q2q2)                            - _2q1 * ax - _2q2 * ay;
```

Remove common factor of 2:

```cpp
s0 = 2 * (q0 * 2 * (q2q2 + q1q1)                            + q2 * ax - q1 * ay);
s1 = 2 * (2 * q1 * (q3q3 + q0q0 - 1 + 2*q1q1 + 2*q2q2 + az) - q3 * ax - q0 * ay);
s2 = 2 * (2 * q2 * (q0q0 + q3q3 - 1 + 2*q1q1 + 2*q2q2 + az) + q0 * ax - q3 * ay);
s3 = 2 * (q3 * 2 * (q1q1 + q2q2)                            - q1 * ax - q2 * ay);
```

Substitute:
`wz_common` = `2 * (q1*q1 + q2*q2)`:

```cpp
s0 = 2 * (q0 * wz_common                              + q2 * ax - q1 * ay);
s1 = 2 * (q1 * 2 * (q3q3 + q0q0 - 1 + wz_common + az) - q3 * ax - q0 * ay);
s2 = 2 * (q2 * 2 * (q0q0 + q3q3 - 1 + wz_common + az) + q0 * ax - q3 * ay);
s3 = 2 * (q3 * wz_common                              - q1 * ax - q2 * ay);
```

Substitute:
`xy_common` = `2 * (q0*q0 + q3*q3 - 1 + wz_common + az)`:

```cpp
s0 = 2 * (q0 * wz_common + q2 * ax - q1 * ay);
s1 = 2 * (q1 * xy_common - q3 * ax - q0 * ay);
s2 = 2 * (q2 * xy_common + q0 * ax - q3 * ay);
s3 = 2 * (q3 * wz_common - q1 * ax - q2 * ay);
```

Instead, calculate half S-values:

```cpp
halfS0 = q0 * wz_common + q2 * ax - q1 * ay;
halfS1 = q1 * xy_common - q3 * ax - q0 * ay;
halfS2 = q2 * xy_common + q0 * ax - q3 * ay;
halfS3 = q3 * wz_common - q1 * ax - q2 * ay;
```

Observe that:
`s0 * invSqrt(s0*s0 + s1*s1 + s2*s2 + s3*s3)` == `halfS0 * invSqrt(halfS0*halfS0 + halfS1*halfS1 + halfS2*halfS2 + halfS3*halfS3)`

(or alternatively note that the norm of a quaternion is the same as the norm of half of that quaternion).
So, for the feedback step, we have:

```cpp
qDot0 -= beta * halfS0 * normReciprocal;
qDot1 -= beta * halfS1 * normReciprocal;
qDot2 -= beta * halfS2 * normReciprocal;
qDot3 -= beta * halfS3 * normReciprocal;
```

Rename `halfS<n>` back to `s<n>`, since `s` is being normalized and they are therefore equivalent:

```cpp
qDot0 -= beta * s0 * normReciprocal;
qDot1 -= beta * s1 * normReciprocal;
qDot2 -= beta * s2 * normReciprocal;
qDot3 -= beta * s3 * normReciprocal;
```

Substituting `betaNormReciprocal` = `beta * reciprocalSqrt(s0*s0 + s1*s1 + s2*s2 + s3*s3)`
we have:

```cpp
qDot0 -= s0 * betaNormReciprocal;
qDot1 -= s1 * betaNormReciprocal;
qDot2 -= s2 * betaNormReciprocal;
qDot3 -= s3 * betaNormReciprocal;
```
