|
Algorithm 1 RFU method with comprehensive metrics. |
|
Require:
|
|
Ensure:
|
| 1: |
initialize
|
| 2: |
while
do
|
| 3: |
calculating the magnetometer measurement-based correction quaternion using Equation (7) |
| 4: |
calculating the accelerometer measurement-based correction quaternion using Equation (6) |
| 5: |
fusing and using Equation (8) |
| 6: |
calculating the main axis’ projections in the reference frame using Equation (9) |
| 7: |
forming the cost function as , and solving it using L-M method. |
| 8: |
end while
|