Better yaw estimation for mulitirotor(magless operation)

[shota3527]

User description

Add a new yaw estimation method for multirotors, which may enable complete magless operation.
The new method gets the acceleration over ground from GPS, then the Z axis of the multirotor should be colinear with the acceleration vector.
https://youtu.be/w23qKWfI2p8

Also some refactoring on the AHRS code, raise ahrs_acc_ignore_rate from 15 to 20 since gps have been better now

---

PR Type

Enhancement

---

Description

This description is generated by an AI tool. It may have inaccuracies

• Add acceleration-based yaw estimation for multirotor magless operation

• Implement GPS acceleration vector for improved heading estimation

• Refactor AHRS code with filtered acceleration measurements

• Increase default accelerometer ignore rate from 15 to 20 degrees/s

---

Diagram Walkthrough

flowchart LR
  GPS["GPS Velocity"] --> AccCalc["Calculate GPS Acceleration"]
  AccCalc --> AccFilter["Filter Acceleration"]
  AccFilter --> YawEst["Yaw Estimation"]
  IMU["IMU Accelerometer"] --> AccFilter
  YawEst --> AHRS["AHRS Update"]
  MAG["Magnetometer"] --> AHRS
  AHRS --> Attitude["Attitude Output"]

Loading

File Walkthrough

	Relevant files
Enhancement	imu.c Implement acceleration-based yaw estimation for multirotors src/main/flight/imu.c Add filtered acceleration measurements with new PT1 filters Implement GPS acceleration-based yaw estimation method Refactor imuMahonyAHRSupdate to support acceleration vectors Add weight calculation for acceleration-based heading correction Improve multirotor heading estimation during acceleration phases +99/-55  imu.h Add filtered acceleration vector export                                    src/main/flight/imu.h Export filtered acceleration vector imuMeasuredAccelBFFiltered +1/-0      realFlight.c Enable GPS velocity components in SITL                                      src/main/target/SITL/sim/realFlight.c Enable GPS velocity components in simulation Fix velocity direction mapping for better GPS data +3/-3
Configuration changes	settings.json Update VSCode file associations                                                    .vscode/settings.json Add maths.h to file associations +2/-1      settings.yaml Increase accelerometer ignore rate threshold                          src/main/fc/settings.yaml Increase default ahrs_acc_ignore_rate from 15 to 20 degrees/s +1/-1

---

[qodo-merge-pro]

PR Reviewer Guide 🔍

Here are some key observations to aid the review process:

⏱️ Estimated effort to review: 4 🔵🔵🔵🔵⚪

⚡ Recommended focus areas for review Possible Issue In imuMahonyAHRSupdate, the GPS yaw weighting adjustment uses the presence of a magnetometer to scale wCoG (if (magBF) { wCoG *= imuConfig()->gps_yaw_weight/100.0f; }). This couples GPS yaw weight to mag availability and may suppress GPS yaw when mag is absent, which is counter to magless operation. Verify intended logic and whether a separate parameter or unconditional GPS scaling is needed. if (magBF || vCOG || vCOGAcc) { float wMag = 1.0f; float wCoG = 1.0f; if (magBF) { wCoG *= imuConfig()->gps_yaw_weight / 100.0f; } fpVector3_t vMagErr = { .v = { 0.0f, 0.0f, 0.0f } }; fpVector3_t vCoGErr = { .v = { 0.0f, 0.0f, 0.0f } }; Filtering Params Accelerometer filtering uses IMU_ROTATION_LPF constants for accelFilterX/Y/Z, which are intended for rotation rates. Using the same cutoff for acceleration may cause suboptimal lag/noise behavior. Consider dedicated accel LPF constants or config to avoid over/under-smoothing. imuMeasuredAccelBFFiltered.x = pt1FilterApply4(&accelFilterX, imuMeasuredAccelBF.x, IMU_ROTATION_LPF, dT); imuMeasuredAccelBFFiltered.y = pt1FilterApply4(&accelFilterY, imuMeasuredAccelBF.y, IMU_ROTATION_LPF, dT); imuMeasuredAccelBFFiltered.z = pt1FilterApply4(&accelFilterZ, imuMeasuredAccelBF.z, IMU_ROTATION_LPF, dT); Coordinate Mapping The simulated GPS velocity components mapping changed to non-zero values with sign adjustments. Ensure the NED/ENU/frame conventions match the consumers (velNED[X/Y/Z]). The comment notes uncertainty for W component; verify axis and sign to prevent heading/acc estimation errors in SITL. (int16_t)roundf(rfValues.m_velocityWorldV_MPS * 100), //direction seems ok (int16_t)roundf(-rfValues.m_velocityWorldU_MPS * 100),//direction seems ok (int16_t)roundf(rfValues.m_velocityWorldW_MPS * 100),//direction not sure 0

[qodo-merge-pro]

PR Code Suggestions ✨

Explore these optional code suggestions:

Category	Suggestion	Impact
High-level	Re-evaluate the core physical assumption The core assumption that a multirotor's Z-axis aligns with its acceleration vector is physically incorrect and should be revised. A multirotor tilts into acceleration, meaning its Z-axis points away from it, which will cause incorrect yaw corrections. Examples: src/main/flight/imu.c [421-478] if (STATE(MULTIROTOR) && (!isMixerTransitionMixing)){ vForward.z = 1.0f; }else{ vForward.x = 1.0f; } fpVector3_t vHeadingEF; // Rotate Forward vector from BF to EF - will yield Heading vector in Earth frame quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation); if (vCOG) { //capital O in COG LOG_DEBUG(IMU, "vCOG=(%f,%f,%f)", (double)vCOG->x, (double)vCOG->y, (double)vCOG->z); ... (clipped 48 lines) Solution Walkthrough: Before: // in imuMahonyAHRSupdate ... // For multirotors, the "forward" vector is the body's Z-axis if (STATE(MULTIROTOR)) { vForward.z = 1.0f; } ... // Rotate body's "forward" vector to Earth Frame to get estimated heading quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation); // If accelerating, use GPS acceleration vector (vCOGAcc) as reference if (wCoGAcc > wCoG) { wCoG = wCoGAcc; vCoGlocal = *vCOGAcc; } ... // Calculate error by comparing estimated heading with reference vector // This tries to align the Z-axis projection with the acceleration vector vectorCrossProduct(&vCoGErr, &vCoGlocal, &vHeadingEF); ... After: // in imuMahonyAHRSupdate ... // For multirotors, the forward vector is the body's X-axis if (STATE(MULTIROTOR)) { vForward.x = 1.0f; } ... // Rotate body's forward vector to Earth Frame to get estimated heading quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation); // The reference vector should be the velocity vector (vCOG), not acceleration. // The acceleration-based logic should be removed or fundamentally redesigned. if (vCOG) { vCoGlocal = *vCOG; // ... apply weight based on speed, etc. } ... // Calculate error by comparing estimated heading with velocity vector // This tries to align the X-axis with the direction of motion. vectorCrossProduct(&vCoGErr, &vCoGlocal, &vHeadingEF); ... Suggestion importance[1-10]: 10 __ Why: The suggestion correctly identifies a critical flaw in the physical model of the new yaw estimation method, which would cause incorrect yaw corrections and likely lead to instability, invalidating the core feature.	High
Possible issue	✅ Prevent division by zero during normalization Suggestion Impact: The commit added an additional condition to the existing check, ensuring vectorNormSquared(&vCoGlocal) > 0.01f before normalizing, which prevents potential division-by-zero. code diff: - if (vectorNormSquared(&vHeadingEF) > 0.01f) { + if (vectorNormSquared(&vHeadingEF) > 0.01f && vectorNormSquared(&vCoGlocal) > 0.01f) { // Normalize to unit vector vectorNormalize(&vHeadingEF, &vHeadingEF); vectorNormalize(&vCoGlocal, &vCoGlocal); Add a check to ensure vCoGlocal has a non-zero magnitude before normalization to prevent a potential division-by-zero error. src/main/flight/imu.c [472-482] -if (vectorNormSquared(&vHeadingEF) > 0.01f) { +if (vectorNormSquared(&vHeadingEF) > 0.01f && vectorNormSquared(&vCoGlocal) > 0.01f) { // Normalize to unit vector vectorNormalize(&vHeadingEF, &vHeadingEF); vectorNormalize(&vCoGlocal, &vCoGlocal); // error is cross product between reference heading and estimated heading (calculated in EF) vectorCrossProduct(&vCoGErr, &vCoGlocal, &vHeadingEF); // Rotate error back into body frame quaternionRotateVector(&vCoGErr, &vCoGErr, &orientation); } [Suggestion processed] Suggestion importance[1-10]: 9 __ Why: The suggestion correctly identifies a potential division-by-zero bug when vCoGlocal is a zero vector, which can occur if there is no GPS-measured acceleration. This prevents NaN propagation into the attitude estimation, which is a critical issue.	High
	Correct vertical acceleration weight calculation Correct the vertical acceleration weight calculation by negating accBFNorm.z to account for the Z-down body frame convention. src/main/flight/imu.c [357-363] static float imuCalculateMcCogAccWeight(void) { fpVector3_t accBFNorm; vectorScale(&accBFNorm, &imuMeasuredAccelBFFiltered, 1.0f / GRAVITY_CMSS); - float wCoGAcc = constrainf((accBFNorm.z - 1.0f)* 2, 0.0f, 1.0f); + float wCoGAcc = constrainf((-accBFNorm.z - 1.0f)* 2, 0.0f, 1.0f); return wCoGAcc; } Apply / Chat Suggestion importance[1-10]: 9 __ Why: The suggestion correctly identifies a logic error in calculating wCoGAcc due to the Z-axis convention in the body frame. The proposed fix is essential for the new GPS acceleration-based heading feature to work as intended.	High
	Fix logic to allow GPS acceleration-based heading Remove the else block that incorrectly resets wCoG to zero, to ensure the GPS acceleration-based heading logic can function correctly. src/main/flight/imu.c [429-446] if (vCOG) { //capital O in COG LOG_DEBUG(IMU, "vCOG=(%f,%f,%f)", (double)vCOG->x, (double)vCOG->y, (double)vCOG->z); vCoGlocal = *vCOG; float airSpeed = gpsSol.groundSpeed; #if defined(USE_WIND_ESTIMATOR) // remove wind elements in vCoGlocal for better heading estimation if (isEstimatedWindSpeedValid() && imuConfig()->gps_yaw_windcomp) { vectorScale(&vCoGlocal, &vCoGlocal, gpsSol.groundSpeed); vCoGlocal.x += getEstimatedWindSpeed(X); vCoGlocal.y -= getEstimatedWindSpeed(Y); airSpeed = fast_fsqrtf(vectorNormSquared(&vCoGlocal)); } #endif wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1000), 400, 1000, 0.0f, 1.0f); -} else { //then vCOGAcc is not null - wCoG = 0.0f; } Apply / Chat Suggestion importance[1-10]: 8 __ Why: The suggestion correctly points out a logical flaw where wCoG is incorrectly zeroed, which would disable the GPS acceleration-based heading correction. Fixing this is important for the new feature's functionality.	Medium
Update

[Jetrell]

since gps have been better now

Over the last years of my testing with INAV 7.1 through to 8.1. Using a wide range of GPS modules. Which include quality M8, M9 and a broad quality range of M10's. I've notice that GPS is not really any better in general.. Or at least its handling for INAV copter use is far from as consistent as it used to be, with the same higher quality M8 modules, and most of the cheaper M10 modules.
The only modules that seem to maintain constant precision, and hold a sold satellite fix, without HDOP/EPH fluctuations, are the M9 and higher end M10's with a larger patch antenna. They generally always make their copters perform well in nav modes.

The reason I raise this matter. Is because leaning too heavily on GNSS data may work fine for those high end modules. But it ends up making copter flight with the cheaper M10 modules, very hit and miss. i.e. some days these copter works well in navigation modes. Other days I just pack up and go home, because the same copter can't do anything right.

So what concerns me with using GPS acceleration. Is the GPS data delay, and wild fluctuations in satellite precision with the older M8's and cheaper M10's; which totally mess-up the estimation for a time. This will prevent lower-end GNSS modules, or poorly layout builds, from being able to obtain good enough acceleration correction data for this feature to work under most conditions.

But I'm also not ruling out something else being wrong with INAV GNSS handling. Whether at the serial communications end or Ublox handling.
I can't put my finger on the cause. But INAV 8.0 has seen many inconstancy, reported by many users in this area.
e.g.

• Module reset of hot start data at each boot, without power cycle or even with a power cycle of only a few seconds, And that after having a high sat count. This one is more random, and occur across a wide range of new and old modules. But occurs with too much regularity to dismiss.

• Random GPS serial com errors, even for a split second, causing emergency landing.

• Wild swings in GPS speed, caused by the loss of a few prominent satellites during banking turns or slow-down; even when the sat count is in the high teen or low twenties. With this same situation effecting altitude estimation.

• Copters and Planes immediately after a 90 deg turn, and straightening up, will pull to the left. When they should be flying straight on the new heading. This occurs when running missions or RTH without a compass. Yawing copters more than 90degs with the stick generally fixes the effect. This one has been there since the changes you made back in 7.1.

[shota3527]

GPS data delay, and wild fluctuations in satellite will mess up the estimation.
But from 6.0, GPS acceleration is used for AHRS calculation, the ahrs_inertia_comp_method=velned. To subtract the centrifugal force from ACC reading, and it had success on fixedwing, not obvious changes on multirotor. So i think this PR also have a chance to work.

Copters and Planes immediately after a 90 deg turn, and straightening up, will pull to the left
any dvr for this?

[Jetrell]

GPS data delay, and wild fluctuations in satellite will mess up the estimation. But from 6.0, GPS acceleration is used for AHRS calculation, the ahrs_inertia_comp_method=velned. To subtract the centrifugal force from ACC reading, and it had success on fixedwing, not obvious changes on multirotor. So i think this PR also have a chance to work.

I agree. It had great success for fixedwings in 6.0 up to 7.1.. But in 8.0 things don't work as well. There have been multiple reports of horizon drift with the AHI. I've also experienced it on the random occasions on several very well setup (prop/motor balanced) and well filtered fixedwings.
Which makes me think it is GPS velocity related. Because it occurs at the times EPH is fluctuating. Bur takes a lot long to correct itself than it did in 6.0, when you first implemented it.
8.0 seen many changes to GPS handling. It also seen features like Dead reckoning and Geofencing added.

Copters and Planes immediately after a 90 deg turn, and straightening up, will pull to the left
any dvr for this?

Not on me without looking through old video file tests.
But have a look over this DVR and log. And read what I wrote in italics about a 180deg turns causing this heading drift. That too was what Breadoven experienced, which lead him to write that PR.. But the cause is still unknown.

Also read what I wrote in the later comments in that PR, after more testing. The effect I wrote of for a copter without a mag, also seems to effect copters with a mag on those occasions.

[shota3527]

When debugging yaw, heading estimation, please pay attention to the goggles feed vs heading number on OSD or the home arrow. Do not rely on navigation modes. The navigation mode complicates the determination of whether it is an estimation problem or a navigation problem.

[sensei-hacker]

Yeah I have also noticed what seems to be more GPS complaints under INAV 8 than INAV 7. I haven't pinned down anything specific, though.

I know that a 180° turn will mess up the AHI when using a traditional mechanical gyro. Continuing through 360° will fix it because the second 180° causes an equal and opposite error. It's not clear to me whether or not the same effect would apply with an MEMS gyro.

[shota3527]

The 180/90 gimbal lock problem will not exist on the Quaternion/Rotation matrix-based AHRS.
In Inav, it computes using the Quaternion representation for core calculation, then converts to the Rotation matrix representation, and finally converts to the RPY representation.

[sensei-hacker]

@shota3527 The AI bot highlighted about 5 things it thought might be issues.
Of course the AI bot is a dumb computer, it doesn't actually know anything and all five of those may be incorrect.

Have you had a chance to review those to see if one or two of the five might actually be real?

[shota3527]

The AI bot highlighted about 5 things it thought might be issues.
reviewed them, and have update one of the issue and update the comments for one of the issue

[qodo-merge-pro]

PR Compliance Guide 🔍

Below is a summary of compliance checks for this PR:

Security Compliance
⚪	Excessive debug logging Description: Debug logging of raw GPS-derived vectors and internal filter states via LOG_DEBUG may expose sensitive telemetry or performance data if left enabled in production builds. imu.c [429-469] Referred Code if (vCOG) { LOG_DEBUG(IMU, "vCOG=(%f,%f,%f)", (double)vCOG->x, (double)vCOG->y, (double)vCOG->z); vCoGlocal = *vCOG; float airSpeed = gpsSol.groundSpeed; #if defined(USE_WIND_ESTIMATOR) // remove wind elements in vCoGlocal for better heading estimation if (isEstimatedWindSpeedValid() && imuConfig()->gps_yaw_windcomp) { vectorScale(&vCoGlocal, &vCoGlocal, gpsSol.groundSpeed); vCoGlocal.x += getEstimatedWindSpeed(X); vCoGlocal.y -= getEstimatedWindSpeed(Y); airSpeed = fast_fsqrtf(vectorNormSquared(&vCoGlocal)); } #endif wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1000), 400, 1000, 0.0f, 1.0f); } else { //vCOG is not avaliable and vCOGAcc is avaliable, set the weight of vCOG to zero wCoG = 0.0f; } if (STATE(MULTIROTOR)){ //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading wCoG *= imuCalculateMcCogWeight(); ... (clipped 20 lines)
	Coordinate mismatch Description: Potential axis/sign mismatch when mapping simulator world velocities to GPS NED components could cause incorrect acceleration/yaw estimation leading to loss of control. realFlight.c [347-350] Referred Code (int16_t)roundf(rfValues.m_velocityWorldV_MPS * 100), //direction seems ok (int16_t)roundf(-rfValues.m_velocityWorldU_MPS * 100),//direction seems ok (int16_t)roundf(rfValues.m_velocityWorldW_MPS * 100),//direction not sure 0
Ticket Compliance
⚪	🎫 No ticket provided Create ticket/issue
Codebase Duplication Compliance
⚪	Codebase context is not defined Follow the guide to enable codebase context checks.
Custom Compliance
🟢	Generic: Secure Error Handling Objective: To prevent the leakage of sensitive system information through error messages while providing sufficient detail for internal debugging. Status: Passed
	Generic: Secure Logging Practices Objective: To ensure logs are useful for debugging and auditing without exposing sensitive information like PII, PHI, or cardholder data. Status: Passed
	Generic: Security-First Input Validation and Data Handling Objective: Ensure all data inputs are validated, sanitized, and handled securely to prevent vulnerabilities Status: Passed
⚪	Generic: Comprehensive Audit Trails Objective: To create a detailed and reliable record of critical system actions for security analysis and compliance. Status: Runtime Logging: The new code adds several LOG_DEBUG statements for IMU/GPS vectors and weights but it is unclear whether these logs meet structured audit requirements or are appropriately scoped to debug-only builds. Referred Code LOG_DEBUG(IMU, "vCOG=(%f,%f,%f)", (double)vCOG->x, (double)vCOG->y, (double)vCOG->z); vCoGlocal = *vCOG; float airSpeed = gpsSol.groundSpeed; #if defined(USE_WIND_ESTIMATOR) // remove wind elements in vCoGlocal for better heading estimation if (isEstimatedWindSpeedValid() && imuConfig()->gps_yaw_windcomp) { vectorScale(&vCoGlocal, &vCoGlocal, gpsSol.groundSpeed); vCoGlocal.x += getEstimatedWindSpeed(X); vCoGlocal.y -= getEstimatedWindSpeed(Y); airSpeed = fast_fsqrtf(vectorNormSquared(&vCoGlocal)); } #endif wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1000), 400, 1000, 0.0f, 1.0f); } else { //vCOG is not avaliable and vCOGAcc is avaliable, set the weight of vCOG to zero wCoG = 0.0f; } if (STATE(MULTIROTOR)){ //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading wCoG *= imuCalculateMcCogWeight(); //handle acc based vector ... (clipped 19 lines)
	Generic: Meaningful Naming and Self-Documenting Code Objective: Ensure all identifiers clearly express their purpose and intent, making code self-documenting Status: Abbreviation Use: New identifiers like vCOG, vCOGAcc, wCoG, and acc_ignore_slope_multipiler use abbreviations and a misspelling that may reduce clarity without nearby documentation. Referred Code static float imuCalculateMcCogWeight(void) { float wCoG = imuCalculateAccelerometerWeightNearness(&imuMeasuredAccelBFFiltered); float rotRateMagnitude = fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBFFiltered)); const float rateSlopeMax = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate)) * 4.0f; wCoG *= scaleRangef(constrainf(rotRateMagnitude, 0.0f, rateSlopeMax), 0.0f, rateSlopeMax, 1.0f, 0.0f); return wCoG; } static float imuCalculateMcCogAccWeight(void) { fpVector3_t accBFNorm; vectorScale(&accBFNorm, &imuMeasuredAccelBFFiltered, 1.0f / GRAVITY_CMSS); float wCoGAcc = constrainf((accBFNorm.z - 1.0f)* 2, 0.0f, 1.0f); //z direction is verified via SITL return wCoGAcc; } static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, const fpVector3_t * vCOG, const fpVector3_t * vCOGAcc, float accWScaler, float magWScaler) { STATIC_FASTRAM fpVector3_t vGyroDriftEstimate = { 0 }; fpQuaternion_t prevOrientation = orientation; fpVector3_t vRotation = *gyroBF; ... (clipped 337 lines)
	Generic: Robust Error Handling and Edge Case Management Objective: Ensure comprehensive error handling that provides meaningful context and graceful degradation Status: Input Bounds: The GPS-derived acceleration computation assumes valid timing and velocity updates without explicit handling for zero/very small dt or missing/invalid GPS velocity components beyond heartbeat checks. Referred Code const fpVector3_t currentGPSvel = {.v = {gpsSol.velNED[X], gpsSol.velNED[Y], gpsSol.velNED[Z]}}; // cm/s gps speed const rtcTime_t currenttime = millis(); // on first gps data acquired, time_delta_ms will be large, vEstcentrifugalAccelBF will be minimal to disable the compensation rtcTime_t time_delta_ms = currenttime - lastGPSNewDataTime; if (lastGPSHeartbeat != gpsSol.flags.gpsHeartbeat && time_delta_ms > 0) { // on new gps frame, update accEF and estimate centrifugal accleration vEstAccelEF->x = -(currentGPSvel.x - lastGPSvel.x) / (MS2S(time_delta_ms)); // the x axis of accerometer is pointing backward vEstAccelEF->y = (currentGPSvel.y - lastGPSvel.y) / (MS2S(time_delta_ms)); vEstAccelEF->z = (currentGPSvel.z - lastGPSvel.z) / (MS2S(time_delta_ms)); // Calculate estimated centrifugal accleration vector in body frame quaternionRotateVector(vEstcentrifugalAccelBF, vEstAccelEF, &orientation); // EF -> BF lastGPSNewDataTime = currenttime; lastGPSvel = currentGPSvel; } lastGPSHeartbeat = gpsSol.flags.gpsHeartbeat; *acc_ignore_slope_multipiler = 4.0f; }

Compliance status legend

🟢 - Fully Compliant
🟡 - Partial Compliant
🔴 - Not Compliant
⚪ - Requires Further Human Verification
🏷️ - Compliance label

[shota3527]

@sensei-hacker
Hi, this PR is not ready yet.
not tested in realflight and some debug printing need clean up. maybe revert the merge for now..

[sensei-hacker]

Do you want expect Realflight testing to be done in the next couple days, or will this need to wait for the next release?

[shota3527]

I will try sqeeze some time to do real flight testing tomorrow. maybe someone else could help?

[Jetrell]

I only got to run some short tests. But it wasn't totally conclusive because its very windy here. And it was also with the copter I had with me on the day. Which happens to have a high quality GNSS module.

I would like experience how these commits would go on a poorer build with a cheaper GNSS module. And on a day that isn't so windy.