Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

1128 lines
54 KiB
C++
Raw Normal View History

AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL/AP_HAL.h>
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
#include "AP_NavEKF2.h"
#include "AP_NavEKF2_core.h"
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Vehicle/AP_Vehicle.h>
AP_NavEKF2: use mavlink messages not printf for state change notifications
2016-07-18 23:16:25 -03:00
#include <GCS_MAVLink/GCS.h>
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
#include <stdio.h>
extern const AP_HAL::HAL& hal;
/********************************************************
* RESET FUNCTIONS *
********************************************************/
// Control reset of yaw and magnetic field states
void NavEKF2_core::controlMagYawReset()
{
AP_NavEKF2: Fix plane in-flight yaw reset bug Fixes bugs that prevented planes being able to reset yaw to GPS to recovery from takeoff with a bad magnetoemter. 1) If the velocity innovation check had not failed by the time the in-air transition occurred, then the yaw reset would not be performed 2) The velocity states were not being reset 3) The non fly-forward vehicle (copter) reset could occur first and effectively lock out the fly-forward vehicle (plane) yaw check.
2016-07-02 09:44:03 -03:00
// Vehicles that can use a zero sideslip assumption (Planes) are a special case
// They can use the GPS velocity to recover from bad initial compass data
// This allows recovery for heading alignment errors due to compass faults
if (assume_zero_sideslip() && !finalInflightYawInit && inFlight ) {
gpsYawResetRequest = true;
return;
AP_NavEKF2: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset.
2016-07-18 09:19:53 -03:00
} else {
gpsYawResetRequest = false;
AP_NavEKF2: Fix plane in-flight yaw reset bug Fixes bugs that prevented planes being able to reset yaw to GPS to recovery from takeoff with a bad magnetoemter. 1) If the velocity innovation check had not failed by the time the in-air transition occurred, then the yaw reset would not be performed 2) The velocity states were not being reset 3) The non fly-forward vehicle (copter) reset could occur first and effectively lock out the fly-forward vehicle (plane) yaw check.
2016-07-02 09:44:03 -03:00
}
AP_NavEKF2: Improve ground based magnetic anomaly protection for copter The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe. A check for yaw errors due to a ground based magnetic anomaly has been introduced. The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
2016-06-09 20:19:28 -03:00
// Quaternion and delta rotation vector that are re-used for different calculations
Vector3f deltaRotVecTemp;
Quaternion deltaQuatTemp;
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
AP_NavEKF2: Improve ground based magnetic anomaly protection for copter The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe. A check for yaw errors due to a ground based magnetic anomaly has been introduced. The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
2016-06-09 20:19:28 -03:00
bool flightResetAllowed = false;
bool initialResetAllowed = false;
if (!finalInflightYawInit) {
// Use a quaternion division to calculate the delta quaternion between the rotation at the current and last time
deltaQuatTemp = stateStruct.quat / prevQuatMagReset;
prevQuatMagReset = stateStruct.quat;
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
AP_NavEKF2: Improve ground based magnetic anomaly protection for copter The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe. A check for yaw errors due to a ground based magnetic anomaly has been introduced. The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
2016-06-09 20:19:28 -03:00
// convert the quaternion to a rotation vector and find its length
deltaQuatTemp.to_axis_angle(deltaRotVecTemp);
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
AP_NavEKF2: Improve ground based magnetic anomaly protection for copter The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe. A check for yaw errors due to a ground based magnetic anomaly has been introduced. The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
2016-06-09 20:19:28 -03:00
// check if the spin rate is OK - high spin rates can cause angular alignment errors
bool angRateOK = deltaRotVecTemp.length() < 0.1745f;
initialResetAllowed = angRateOK;
flightResetAllowed = angRateOK && !onGround;
}
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
AP_NavEKF2: remove duplicate if statements from mag fusion
2016-06-25 08:07:35 -03:00
// Check if conditions for a interim or final yaw/mag reset are met
AP_NavEKF2: Improve ground based magnetic anomaly protection for copter The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe. A check for yaw errors due to a ground based magnetic anomaly has been introduced. The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
2016-06-09 20:19:28 -03:00
bool finalResetRequest = false;
AP_NavEKF2: remove duplicate if statements from mag fusion
2016-06-25 08:07:35 -03:00
bool interimResetRequest = false;
AP_NavEKF2: Improve ground based magnetic anomaly protection for copter The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe. A check for yaw errors due to a ground based magnetic anomaly has been introduced. The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
2016-06-09 20:19:28 -03:00
if (flightResetAllowed && !assume_zero_sideslip()) {
AP_NavEKF2: remove duplicate if statements from mag fusion
2016-06-25 08:07:35 -03:00
// check that we have reached a height where ground magnetic interference effects are insignificant
// and can perform a final reset of the yaw and field states
AP_NavEKF2: Improve ground based magnetic anomaly protection for copter The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe. A check for yaw errors due to a ground based magnetic anomaly has been introduced. The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
2016-06-09 20:19:28 -03:00
finalResetRequest = (stateStruct.position.z - posDownAtTakeoff) < -5.0f;
// check for increasing height
bool hgtIncreasing = (posDownAtLastMagReset-stateStruct.position.z) > 0.5f;
float yawInnovIncrease = fabsf(innovYaw) - fabsf(yawInnovAtLastMagReset);
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
AP_NavEKF2: Improve ground based magnetic anomaly protection for copter The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe. A check for yaw errors due to a ground based magnetic anomaly has been introduced. The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
2016-06-09 20:19:28 -03:00
// check for increasing yaw innovations
bool yawInnovIncreasing = yawInnovIncrease > 0.25f;
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
AP_NavEKF2: Improve ground based magnetic anomaly protection for copter The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe. A check for yaw errors due to a ground based magnetic anomaly has been introduced. The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
2016-06-09 20:19:28 -03:00
// check that the yaw innovations haven't been caused by a large change in attitude
deltaQuatTemp = quatAtLastMagReset / stateStruct.quat;
deltaQuatTemp.to_axis_angle(deltaRotVecTemp);
bool largeAngleChange = deltaRotVecTemp.length() > yawInnovIncrease;
// if yaw innovations and height have increased and we haven't rotated much
// then we are climbing away from a ground based magnetic anomaly and need to reset
interimResetRequest = hgtIncreasing && yawInnovIncreasing && !largeAngleChange;
}
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
// an initial reset is required if we have not yet aligned the yaw angle
AP_NavEKF2: Improve ground based magnetic anomaly protection for copter The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe. A check for yaw errors due to a ground based magnetic anomaly has been introduced. The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
2016-06-09 20:19:28 -03:00
bool initialResetRequest = initialResetAllowed && !yawAlignComplete;
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
// a combined yaw angle and magnetic field reset can be initiated by:
magYawResetRequest = magYawResetRequest || // an external request
AP_NavEKF2: Improve ground based magnetic anomaly protection for copter The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe. A check for yaw errors due to a ground based magnetic anomaly has been introduced. The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
2016-06-09 20:19:28 -03:00
initialResetRequest || // an initial alignment performed by all vehicle types using magnetometer
interimResetRequest || // an interim alignment required to recover from ground based magnetic anomaly
finalResetRequest; // the final reset when we have acheived enough height to be in stable magnetic field environment
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
// Perform a reset of magnetic field states and reset yaw to corrected magnetic heading
if (magYawResetRequest || magStateResetRequest) {
AP_NavEKF2: Improve ground based magnetic anomaly protection for copter The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe. A check for yaw errors due to a ground based magnetic anomaly has been introduced. The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
2016-06-09 20:19:28 -03:00
// get the euler angles from the current state estimate
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
Vector3f eulerAngles;
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
// Use the Euler angles and magnetometer measurement to update the magnetic field states
// and get an updated quaternion
Quaternion newQuat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
AP_NavEKF2: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset.
2016-07-18 09:19:53 -03:00
// if a yaw reset has been requested, apply the updated quaternion to the current state
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
if (magYawResetRequest) {
// previous value used to calculate a reset delta
Quaternion prevQuat = stateStruct.quat;
// update the quaternion states using the new yaw angle
stateStruct.quat = newQuat;
AP_NavEKF2: Improve protection against ground based mag interference Reset the mag field states and yaw earlier than the normal 5m height threshold if toilet bowling is detected.
2016-03-16 19:16:37 -03:00
// calculate the change in the quaternion state and apply it to the ouput history buffer
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
prevQuat = stateStruct.quat/prevQuat;
StoreQuatRotate(prevQuat);
// send initial alignment status to console
if (!yawAlignComplete) {
AP_NavEKF2: change priority of statustext messages
2016-07-28 18:25:07 -03:00
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u initial yaw alignment complete",(unsigned)imu_index);
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
}
AP_NavEKF2: Improve ground based magnetic anomaly protection for copter The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe. A check for yaw errors due to a ground based magnetic anomaly has been introduced. The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
2016-06-09 20:19:28 -03:00
// send in-flight yaw alignment status to console
if (finalResetRequest) {
AP_NavEKF2: change priority of statustext messages
2016-07-28 18:25:07 -03:00
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u in-flight yaw alignment complete",(unsigned)imu_index);
AP_NavEKF2: Improve ground based magnetic anomaly protection for copter The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe. A check for yaw errors due to a ground based magnetic anomaly has been introduced. The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
2016-06-09 20:19:28 -03:00
} else if (interimResetRequest) {
AP_NavEKF2: use mavlink messages not printf for state change notifications
2016-07-18 23:16:25 -03:00
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF2 IMU%u ground mag anomaly, yaw re-aligned",(unsigned)imu_index);
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
}
// update the yaw reset completed status
recordYawReset();
// clear the yaw reset request flag
magYawResetRequest = false;
AP_NavEKF2: Improve ground based magnetic anomaly protection for copter The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe. A check for yaw errors due to a ground based magnetic anomaly has been introduced. The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
2016-06-09 20:19:28 -03:00
// clear the complete flags if an interim reset has been performed to allow subsequent
// and final reset to occur
if (interimResetRequest) {
finalInflightYawInit = false;
finalInflightMagInit = false;
}
AP_NavEKF2: Improve protection against ground based mag interference Reset the mag field states and yaw earlier than the normal 5m height threshold if toilet bowling is detected.
2016-03-16 19:16:37 -03:00
}
AP_NavEKF2: Improve ground based magnetic anomaly protection for copter The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe. A check for yaw errors due to a ground based magnetic anomaly has been introduced. The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
2016-06-09 20:19:28 -03:00
}
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
}
AP_NavEKF2: update function name The primary purpose of this function is re-alignment
2016-05-20 22:13:03 -03:00
// this function is used to do a forced re-alignment of the yaw angle to align with the horizontal velocity
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
// vector from GPS. It is used to align the yaw angle after launch or takeoff.
AP_NavEKF2: update function name The primary purpose of this function is re-alignment
2016-05-20 22:13:03 -03:00
void NavEKF2_core::realignYawGPS()
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
{
if ((sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y)) > 25.0f) {
AP_NavEKF2: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset.
2016-07-18 09:19:53 -03:00
// get quaternion from existing filter states and calculate roll, pitch and yaw angles
Vector3f eulerAngles;
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
// calculate course yaw angle
AP_NavEKF2: Fix plane in-flight yaw reset The copter method was being used for plane and the plane method was not being run due to the change in flight status not being detected. The plane reset method did not trigger if the EKF had already dragged the velocity states along with the GPS or could align to an incorrect heading. The method has been reworked so that it resets to the GPS course, but only if there are inconsistent angles and large innovations. To stop a failed magnetometer causing a loss of yaw reference later in flight, if all available sensors have been tried in flight and timed out, then no further magnetoemter data will be used
2015-11-15 17:05:08 -04:00
float velYaw = atan2f(stateStruct.velocity.y,stateStruct.velocity.x);
// calculate course yaw angle from GPS velocity
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
float gpsYaw = atan2f(gpsDataDelayed.vel.y,gpsDataDelayed.vel.x);
AP_NavEKF2: Update GPS yaw re-alignment function Updates to support initial alignment without magnetometer
2016-05-20 22:48:58 -03:00
AP_NavEKF2: Fix plane in-flight yaw reset The copter method was being used for plane and the plane method was not being run due to the change in flight status not being detected. The plane reset method did not trigger if the EKF had already dragged the velocity states along with the GPS or could align to an incorrect heading. The method has been reworked so that it resets to the GPS course, but only if there are inconsistent angles and large innovations. To stop a failed magnetometer causing a loss of yaw reference later in flight, if all available sensors have been tried in flight and timed out, then no further magnetoemter data will be used
2015-11-15 17:05:08 -04:00
// Check the yaw angles for consistency
AP_NavEKF2: Fix plane in-flight yaw reset bug Fixes bugs that prevented planes being able to reset yaw to GPS to recovery from takeoff with a bad magnetoemter. 1) If the velocity innovation check had not failed by the time the in-air transition occurred, then the yaw reset would not be performed 2) The velocity states were not being reset 3) The non fly-forward vehicle (copter) reset could occur first and effectively lock out the fly-forward vehicle (plane) yaw check.
2016-07-02 09:44:03 -03:00
float yawErr = MAX(fabsf(wrap_PI(gpsYaw - velYaw)),fabsf(wrap_PI(gpsYaw - eulerAngles.z)));
AP_NavEKF2: Fix plane in-flight yaw reset The copter method was being used for plane and the plane method was not being run due to the change in flight status not being detected. The plane reset method did not trigger if the EKF had already dragged the velocity states along with the GPS or could align to an incorrect heading. The method has been reworked so that it resets to the GPS course, but only if there are inconsistent angles and large innovations. To stop a failed magnetometer causing a loss of yaw reference later in flight, if all available sensors have been tried in flight and timed out, then no further magnetoemter data will be used
2015-11-15 17:05:08 -04:00
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
// If the angles disagree by more than 45 degrees and GPS innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad
badMagYaw = ((yawErr > 0.7854f) && (velTestRatio > 1.0f) && (PV_AidingMode == AID_ABSOLUTE)) || !yawAlignComplete;
AP_NavEKF2: Fix plane in-flight yaw reset The copter method was being used for plane and the plane method was not being run due to the change in flight status not being detected. The plane reset method did not trigger if the EKF had already dragged the velocity states along with the GPS or could align to an incorrect heading. The method has been reworked so that it resets to the GPS course, but only if there are inconsistent angles and large innovations. To stop a failed magnetometer causing a loss of yaw reference later in flight, if all available sensors have been tried in flight and timed out, then no further magnetoemter data will be used
2015-11-15 17:05:08 -04:00
AP_NavEKF2: Allow use in planes without a magnetometer Implements the following techniques to enable planes to operate without magnetometers. 1) When on ground with mag use inhibited, a synthetic heading equal to current heading is fused to prevent uncontrolled covariance growth. 2) When transitioning to in-flight, the delta between inertial and GPS velocity vector is used to align the yaw. 3) The yaw gyro bias state variance is reset following an in-flight heading reset to enable the yaw gyro bias to be learned faster.
2016-04-08 02:35:21 -03:00
// correct yaw angle using GPS ground course if compass yaw bad
AP_NavEKF2: Fix plane in-flight yaw reset The copter method was being used for plane and the plane method was not being run due to the change in flight status not being detected. The plane reset method did not trigger if the EKF had already dragged the velocity states along with the GPS or could align to an incorrect heading. The method has been reworked so that it resets to the GPS course, but only if there are inconsistent angles and large innovations. To stop a failed magnetometer causing a loss of yaw reference later in flight, if all available sensors have been tried in flight and timed out, then no further magnetoemter data will be used
2015-11-15 17:05:08 -04:00
if (badMagYaw) {
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
// calculate new filter quaternion states from Euler angles
AP_NavEKF2: Fix plane in-flight yaw reset The copter method was being used for plane and the plane method was not being run due to the change in flight status not being detected. The plane reset method did not trigger if the EKF had already dragged the velocity states along with the GPS or could align to an incorrect heading. The method has been reworked so that it resets to the GPS course, but only if there are inconsistent angles and large innovations. To stop a failed magnetometer causing a loss of yaw reference later in flight, if all available sensors have been tried in flight and timed out, then no further magnetoemter data will be used
2015-11-15 17:05:08 -04:00
stateStruct.quat.from_euler(eulerAngles.x, eulerAngles.y, gpsYaw);
AP_NavEKF2: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset.
2016-07-18 09:19:53 -03:00
// reset the velocity and posiiton states as they will be inaccurate due to bad yaw
ResetVelocity();
ResetPosition();
AP_NavEKF2: Fix plane in-flight yaw reset The copter method was being used for plane and the plane method was not being run due to the change in flight status not being detected. The plane reset method did not trigger if the EKF had already dragged the velocity states along with the GPS or could align to an incorrect heading. The method has been reworked so that it resets to the GPS course, but only if there are inconsistent angles and large innovations. To stop a failed magnetometer causing a loss of yaw reference later in flight, if all available sensors have been tried in flight and timed out, then no further magnetoemter data will be used
2015-11-15 17:05:08 -04:00
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
// send yaw alignment information to console
AP_NavEKF2: change priority of statustext messages
2016-07-28 18:25:07 -03:00
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u yaw aligned to GPS velocity",(unsigned)imu_index);
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
AP_NavEKF2: attitude co-variance reset for all post alignment yaw resets Whenever the yaw angle is changed, the correlation with other state errors will be incorrect and the terms should be zeroed.
2016-05-23 22:03:14 -03:00
// zero the attitude covariances becasue the corelations will now be invalid
zeroAttCovOnly();
AP_NavEKF2: Fix plane in-flight yaw reset The copter method was being used for plane and the plane method was not being run due to the change in flight status not being detected. The plane reset method did not trigger if the EKF had already dragged the velocity states along with the GPS or could align to an incorrect heading. The method has been reworked so that it resets to the GPS course, but only if there are inconsistent angles and large innovations. To stop a failed magnetometer causing a loss of yaw reference later in flight, if all available sensors have been tried in flight and timed out, then no further magnetoemter data will be used
2015-11-15 17:05:08 -04:00
AP_NavEKF2: Fix plane in-flight yaw reset bug Fixes bugs that prevented planes being able to reset yaw to GPS to recovery from takeoff with a bad magnetoemter. 1) If the velocity innovation check had not failed by the time the in-air transition occurred, then the yaw reset would not be performed 2) The velocity states were not being reset 3) The non fly-forward vehicle (copter) reset could occur first and effectively lock out the fly-forward vehicle (plane) yaw check.
2016-07-02 09:44:03 -03:00
// record the yaw reset event
recordYawReset();
AP_NavEKF2: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset.
2016-07-18 09:19:53 -03:00
// clear all pending yaw reset requests
AP_NavEKF2: Fix plane in-flight yaw reset bug Fixes bugs that prevented planes being able to reset yaw to GPS to recovery from takeoff with a bad magnetoemter. 1) If the velocity innovation check had not failed by the time the in-air transition occurred, then the yaw reset would not be performed 2) The velocity states were not being reset 3) The non fly-forward vehicle (copter) reset could occur first and effectively lock out the fly-forward vehicle (plane) yaw check.
2016-07-02 09:44:03 -03:00
gpsYawResetRequest = false;
AP_NavEKF2: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset.
2016-07-18 09:19:53 -03:00
magYawResetRequest = false;
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
AP_NavEKF2: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset.
2016-07-18 09:19:53 -03:00
if (use_compass()) {
// request a mag field reset which may enable us to use the magnetoemter if the previous fault was due to bad initialisation
magStateResetRequest = true;
// clear the all sensors failed status so that the magnetometers sensors get a second chance now that we are flying
allMagSensorsFailed = false;
}
AP_NavEKF2: Fix plane in-flight yaw reset bug Fixes bugs that prevented planes being able to reset yaw to GPS to recovery from takeoff with a bad magnetoemter. 1) If the velocity innovation check had not failed by the time the in-air transition occurred, then the yaw reset would not be performed 2) The velocity states were not being reset 3) The non fly-forward vehicle (copter) reset could occur first and effectively lock out the fly-forward vehicle (plane) yaw check.
2016-07-02 09:44:03 -03:00
}
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
}
}
/********************************************************
* FUSE MEASURED_DATA *
********************************************************/
// select fusion of magnetometer data
void NavEKF2_core::SelectMagFusion()
{
// start performance timer
NavEKF2: use new perf counter API
2015-10-20 02:42:50 -03:00
hal.util->perf_begin(_perf_FuseMagnetometer);
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
AP_NavEKF2: Level processor loading between frames Don't fuse other measurements on the same frame that magnetometer measurements arrive if running at a high frame rate as there will be insufficient time to complete other operations.
2015-10-18 19:34:11 -03:00
// clear the flag that lets other processes know that the expensive magnetometer fusion operation has been perfomred on that time step
// used for load levelling
magFusePerformed = false;
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
// check for and read new magnetometer measurements
readMagData();
// If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout
if (magHealth) {
magTimeout = false;
lastHealthyMagTime_ms = imuSampleTime_ms;
AP_NavEKF2: added EK2_IMU_MASK for multiple IMUs no voting between multiple IMUs yet
2015-11-04 21:00:57 -04:00
} else if ((imuSampleTime_ms - lastHealthyMagTime_ms) > frontend->magFailTimeLimit_ms && use_compass()) {
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
magTimeout = true;
}
AP_NavEKF2: Improve initialisation of magnetic field learning Use the more robust, but less accurate compass heading fusion up to 5m altitude Wait for the magnetometer data fusion time offset to be correct before using data to reset states Don't reset magnetic field states if the vehicle is rotating rapidly as timing offsets will produce large errors When doing the yaw angle reset, apply the reset increment to all quaternions stored in the output buffer to avoid transients produced by yaw rotations and the 0.25 second fusion time horizon offset. Only do the one yaw and mag reset at 5m, not two at 1.5 and 5.0m Always re-do the yaw and mag reset when leaving the ground.
2015-10-28 06:06:40 -03:00
// check for availability of magnetometer data to fuse
AP_NavEKF2: measurement buffer refactor
2015-11-13 18:28:45 -04:00
magDataToFuse = storedMag.recall(magDataDelayed,imuDataDelayed.time_ms);
AP_NavEKF2: Improve initialisation of magnetic field learning Use the more robust, but less accurate compass heading fusion up to 5m altitude Wait for the magnetometer data fusion time offset to be correct before using data to reset states Don't reset magnetic field states if the vehicle is rotating rapidly as timing offsets will produce large errors When doing the yaw angle reset, apply the reset increment to all quaternions stored in the output buffer to avoid transients produced by yaw rotations and the 0.25 second fusion time horizon offset. Only do the one yaw and mag reset at 5m, not two at 1.5 and 5.0m Always re-do the yaw and mag reset when leaving the ground.
2015-10-28 06:06:40 -03:00
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
// Control reset of yaw and magnetic field states if we are using compass data
if (magDataToFuse && use_compass()) {
AP_NavEKF2: Improve initialisation of magnetic field learning Use the more robust, but less accurate compass heading fusion up to 5m altitude Wait for the magnetometer data fusion time offset to be correct before using data to reset states Don't reset magnetic field states if the vehicle is rotating rapidly as timing offsets will produce large errors When doing the yaw angle reset, apply the reset increment to all quaternions stored in the output buffer to avoid transients produced by yaw rotations and the 0.25 second fusion time horizon offset. Only do the one yaw and mag reset at 5m, not two at 1.5 and 5.0m Always re-do the yaw and mag reset when leaving the ground.
2015-10-28 06:06:40 -03:00
controlMagYawReset();
}
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
// determine if conditions are right to start a new fusion cycle
// wait until the EKF time horizon catches up with the measurement
AP_NavEKF2: Make magnetometer data available status global This assists with debugging and makes it consistent with other measurements that are buffered
2015-11-12 04:10:09 -04:00
bool dataReady = (magDataToFuse && statesInitialised && use_compass() && yawAlignComplete);
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
if (dataReady) {
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
// use the simple method of declination to maintain heading if we cannot use the magnetic field states
if(inhibitMagStates || magStateResetRequest || !magStateInitComplete) {
AP_NavEKF2: Improved magnetic heading fusion Use an Euler yaw heading that switches between a 321 and 312 rotation sequence to avoid areas of singularity. Using Euler yaw decouples the observation from the roll and pitch states and prevents magnetic disturbances from affecting roll and pitch via the magnetometer fusion process.
2016-02-29 18:51:45 -04:00
fuseEulerYaw();
AP_NavEKF2: reduce declination errors on start of 3-axis fusion Reset co-variances for NE field states.
2016-07-03 08:17:40 -03:00
// zero the test ratio output from the inactive 3-axis magnetometer fusion
AP_NavEKF2: Add improved health monitoring when using simple compass yaw fusion
2015-10-28 22:36:53 -03:00
magTestRatio.zero();
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
} else {
AP_NavEKF2: Allow use of magnetometer learning during optical flow nav Adds fusion of the declination when there are no earth relative measurements so that the declination angle and therefore the copters yaw angle have an absolute reference. This enables the length (but not the declination) of the earth field North/East states to change along with the magnetometer offsets.
2015-10-18 07:16:55 -03:00
// if we are not doing aiding with earth relative observations (eg GPS) then the declination is
// maintained by fusing declination as a synthesised observation
AP_NavEKF2: Improve handling of GPS loss and recovery for planes Extended GPS loss can result in the earth field states becoming rotated and making it difficult for the EKF to recover its heading when GPS is regained. During prolonged GPS outages, the position covariance can become large enough to cause the reset function to continually activate. This is fixed by ensuring that position covariances are always reset when the position is reset. The innovation variance was being used incorrectly instead of the state variance to trigger the glitch reset.
2015-10-23 09:44:43 -03:00
if (PV_AidingMode != AID_ABSOLUTE || (imuSampleTime_ms - lastPosPassTime_ms) > 4000) {
AP_NavEKF2: reduce declination errors on start of 3-axis fusion Reset co-variances for NE field states.
2016-07-03 08:17:40 -03:00
FuseDeclination(0.34f);
AP_NavEKF2: Allow use of magnetometer learning during optical flow nav Adds fusion of the declination when there are no earth relative measurements so that the declination angle and therefore the copters yaw angle have an absolute reference. This enables the length (but not the declination) of the earth field North/East states to change along with the magnetometer offsets.
2015-10-18 07:16:55 -03:00
}
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
// fuse the three magnetometer componenents sequentially
AP_NavEKF2: enable fine grained perf tuning for mag fusion
2015-10-19 18:32:07 -03:00
for (mag_state.obsIndex = 0; mag_state.obsIndex <= 2; mag_state.obsIndex++) {
NavEKF2: use new perf counter API
2015-10-20 02:42:50 -03:00
hal.util->perf_begin(_perf_test[0]);
AP_NavEKF2: enable fine grained perf tuning for mag fusion
2015-10-19 18:32:07 -03:00
FuseMagnetometer();
NavEKF2: use new perf counter API
2015-10-20 02:42:50 -03:00
hal.util->perf_end(_perf_test[0]);
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
// don't continue fusion if unhealthy
if (!magHealth) {
break;
}
AP_NavEKF2: enable fine grained perf tuning for mag fusion
2015-10-19 18:32:07 -03:00
}
AP_NavEKF2: Add improved health monitoring when using simple compass yaw fusion
2015-10-28 22:36:53 -03:00
// zero the test ratio output from the inactive simple magnetometer yaw fusion
yawTestRatio = 0.0f;
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
}
}
AP_NavEKF2: Allow use in planes without a magnetometer Implements the following techniques to enable planes to operate without magnetometers. 1) When on ground with mag use inhibited, a synthetic heading equal to current heading is fused to prevent uncontrolled covariance growth. 2) When transitioning to in-flight, the delta between inertial and GPS velocity vector is used to align the yaw. 3) The yaw gyro bias state variance is reset following an in-flight heading reset to enable the yaw gyro bias to be learned faster.
2016-04-08 02:35:21 -03:00
// If we have no magnetometer and are on the ground, fuse in a synthetic heading measurement to prevent the
// filter covariances from becoming badly conditioned
if (!use_compass()) {
if (onGround && (imuSampleTime_ms - lastSynthYawTime_ms > 1000)) {
fuseEulerYaw();
magTestRatio.zero();
yawTestRatio = 0.0f;
lastSynthYawTime_ms = imuSampleTime_ms;
AP_NavEKF2: move yaw reset for no compass case Should not be in a function that performs reset on magnetic field states
2016-05-23 20:46:56 -03:00
}
AP_NavEKF2: Allow use in planes without a magnetometer Implements the following techniques to enable planes to operate without magnetometers. 1) When on ground with mag use inhibited, a synthetic heading equal to current heading is fused to prevent uncontrolled covariance growth. 2) When transitioning to in-flight, the delta between inertial and GPS velocity vector is used to align the yaw. 3) The yaw gyro bias state variance is reset following an in-flight heading reset to enable the yaw gyro bias to be learned faster.
2016-04-08 02:35:21 -03:00
}
AP_NavEKF2: remember mag field states between flight on same power cycle Remember the mag bias and earth field states learned during flight when the vehicle lands. This improves performance for vehicles that do multiple flight on one power cycle
2016-07-02 05:46:42 -03:00
// If the final yaw reset has been performed and the state variances are sufficiently low
// record that the earth field has been learned.
if (!magFieldLearned && finalInflightMagInit) {
AP_NavEKF2: clean up mag field learned logic
2016-07-10 11:44:33 -03:00
magFieldLearned = (P[16][16] < sq(0.01f)) && (P[17][17] < sq(0.01f)) && (P[18][18] < sq(0.01f));
AP_NavEKF2: remember mag field states between flight on same power cycle Remember the mag bias and earth field states learned during flight when the vehicle lands. This improves performance for vehicles that do multiple flight on one power cycle
2016-07-02 05:46:42 -03:00
}
// record the last learned field variances
if (magFieldLearned && !inhibitMagStates) {
earthMagFieldVar.x = P[16][16];
earthMagFieldVar.y = P[17][17];
earthMagFieldVar.z = P[18][18];
bodyMagFieldVar.x = P[19][19];
bodyMagFieldVar.y = P[20][20];
bodyMagFieldVar.z = P[21][21];
}
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
// stop performance timer
NavEKF2: use new perf counter API
2015-10-20 02:42:50 -03:00
hal.util->perf_end(_perf_FuseMagnetometer);
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
}
AP_NavEKF2: Update function header comments
2015-10-17 20:32:11 -03:00
/*
* Fuse magnetometer measurements using explicit algebraic equations generated with Matlab symbolic toolbox.
* The script file used to generate these and other equations in this filter can be found here:
* https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m
*/
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
void NavEKF2_core::FuseMagnetometer()
{
NavEKF2: use new perf counter API
2015-10-20 02:42:50 -03:00
hal.util->perf_begin(_perf_test[1]);
AP_NavEKF2: enable fine grained perf tuning for mag fusion
2015-10-19 18:32:07 -03:00
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
// declarations
ftype &q0 = mag_state.q0;
ftype &q1 = mag_state.q1;
ftype &q2 = mag_state.q2;
ftype &q3 = mag_state.q3;
ftype &magN = mag_state.magN;
ftype &magE = mag_state.magE;
ftype &magD = mag_state.magD;
ftype &magXbias = mag_state.magXbias;
ftype &magYbias = mag_state.magYbias;
ftype &magZbias = mag_state.magZbias;
uint8_t &obsIndex = mag_state.obsIndex;
Matrix3f &DCM = mag_state.DCM;
Vector3f &MagPred = mag_state.MagPred;
ftype &R_MAG = mag_state.R_MAG;
ftype *SH_MAG = &mag_state.SH_MAG[0];
Vector24 H_MAG;
Vector6 SK_MX;
Vector6 SK_MY;
Vector6 SK_MZ;
NavEKF2: use new perf counter API
2015-10-20 02:42:50 -03:00
hal.util->perf_end(_perf_test[1]);
AP_NavEKF2: enable fine grained perf tuning for mag fusion
2015-10-19 18:32:07 -03:00
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
// perform sequential fusion of magnetometer measurements.
// this assumes that the errors in the different components are
// uncorrelated which is not true, however in the absence of covariance
// data fit is the only assumption we can make
// so we might as well take advantage of the computational efficiencies
// associated with sequential fusion
// calculate observation jacobians and Kalman gains
if (obsIndex == 0)
{
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
NavEKF2: use new perf counter API
2015-10-20 02:42:50 -03:00
hal.util->perf_begin(_perf_test[2]);
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
// copy required states to local variable names
q0 = stateStruct.quat[0];
q1 = stateStruct.quat[1];
q2 = stateStruct.quat[2];
q3 = stateStruct.quat[3];
magN = stateStruct.earth_magfield[0];
magE = stateStruct.earth_magfield[1];
magD = stateStruct.earth_magfield[2];
magXbias = stateStruct.body_magfield[0];
magYbias = stateStruct.body_magfield[1];
magZbias = stateStruct.body_magfield[2];
// rotate predicted earth components into body axes and calculate
// predicted measurements
DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3;
AP_NavEKF2: Explicitly define constants as floats
2015-10-19 05:29:11 -03:00
DCM[0][1] = 2.0f*(q1*q2 + q0*q3);
DCM[0][2] = 2.0f*(q1*q3-q0*q2);
DCM[1][0] = 2.0f*(q1*q2 - q0*q3);
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3;
AP_NavEKF2: Explicitly define constants as floats
2015-10-19 05:29:11 -03:00
DCM[1][2] = 2.0f*(q2*q3 + q0*q1);
DCM[2][0] = 2.0f*(q1*q3 + q0*q2);
DCM[2][1] = 2.0f*(q2*q3 - q0*q1);
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3;
MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE + DCM[0][2]*magD + magXbias;
MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias;
MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias;
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
// calculate the measurement innovation for each axis
for (uint8_t i = 0; i<=2; i++) {
innovMag[i] = MagPred[i] - magDataDelayed.mag[i];
}
AP_NavEKF2: Reduce allowance for magnetometer timing errors The previous gain from rate to magnetometer error was excessive. The revised value is equivalent to a magnetic field length of 0.5 with a timing uncertainty of 0.01 sec
2015-10-24 06:10:20 -03:00
// scale magnetometer observation error with total angular rate to allow for timing errors
AP_NavEKF2: Fix bug in use of corrected IMU data IMU data was being corrected before being used by the co-variance prediction, whereas the delta angles and velocities in the derivation were supposed to be uncorrected. This patch creates separate variable for the corrected data
2016-06-29 09:38:45 -03:00
R_MAG = sq(constrain_float(frontend->_magNoise, 0.01f, 0.5f)) + sq(frontend->magVarRateScale*delAngCorrected.length() / imuDataDelayed.delAngDT);
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
// calculate common expressions used to calculate observation jacobians an innovation variance for each component
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
SH_MAG[1] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
SH_MAG[2] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
AP_NavEKF2: Explicitly define constants as floats
2015-10-19 05:29:11 -03:00
SH_MAG[3] = 2.0f*q0*q1 + 2.0f*q2*q3;
SH_MAG[4] = 2.0f*q0*q3 + 2.0f*q1*q2;
SH_MAG[5] = 2.0f*q0*q2 + 2.0f*q1*q3;
SH_MAG[6] = magE*(2.0f*q0*q1 - 2.0f*q2*q3);
SH_MAG[7] = 2.0f*q1*q3 - 2.0f*q0*q2;
SH_MAG[8] = 2.0f*q0*q3;
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
// Calculate the innovation variance for each axis
// X axis
AP_NavEKF2: Explicitly define constants as floats
2015-10-19 05:29:11 -03:00
varInnovMag[0] = (P[19][19] + R_MAG - P[1][19]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][19]*SH_MAG[1] + P[17][19]*SH_MAG[4] + P[18][19]*SH_MAG[7] + P[2][19]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) - (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[19][1] - P[1][1]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][1]*SH_MAG[1] + P[17][1]*SH_MAG[4] + P[18][1]*SH_MAG[7] + P[2][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + SH_MAG[1]*(P[19][16] - P[1][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][16]*SH_MAG[1] + P[17][16]*SH_MAG[4] + P[18][16]*SH_MAG[7] + P[2][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + SH_MAG[4]*(P[19][17] - P[1][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][17]*SH_MAG[1] + P[17][17]*SH_MAG[4] + P[18][17]*SH_MAG[7] + P[2][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + SH_MAG[7]*(P[19][18] - P[1][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][18]*SH_MAG[1] + P[17][18]*SH_MAG[4] + P[18][18]*SH_MAG[7] + P[2][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))*(P[19][2] - P[1][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][2]*SH_MAG[1] + P[17][2]*SH_MAG[4] + P[18][2]*SH_MAG[7] + P[2][2]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))));
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
if (varInnovMag[0] >= R_MAG) {
faultStatus.bad_xmag = false;
} else {
// the calculation is badly conditioned, so we cannot perform fusion on this step
// we reset the covariance matrix and try again next measurement
CovarianceInit();
obsIndex = 1;
faultStatus.bad_xmag = true;
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
hal.util->perf_end(_perf_test[2]);
return;
}
// Y axis
varInnovMag[1] = (P[20][20] + R_MAG + P[0][20]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][20]*SH_MAG[0] + P[18][20]*SH_MAG[3] - (SH_MAG[8] - 2.0f*q1*q2)*(P[20][16] + P[0][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][16]*SH_MAG[0] + P[18][16]*SH_MAG[3] - P[2][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][16]*(SH_MAG[8] - 2.0f*q1*q2)) - P[2][20]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[20][0] + P[0][0]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][0]*SH_MAG[0] + P[18][0]*SH_MAG[3] - P[2][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][0]*(SH_MAG[8] - 2.0f*q1*q2)) + SH_MAG[0]*(P[20][17] + P[0][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][17]*SH_MAG[0] + P[18][17]*SH_MAG[3] - P[2][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][17]*(SH_MAG[8] - 2.0f*q1*q2)) + SH_MAG[3]*(P[20][18] + P[0][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][18]*SH_MAG[0] + P[18][18]*SH_MAG[3] - P[2][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][18]*(SH_MAG[8] - 2.0f*q1*q2)) - P[16][20]*(SH_MAG[8] - 2.0f*q1*q2) - (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[20][2] + P[0][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][2]*SH_MAG[0] + P[18][2]*SH_MAG[3] - P[2][2]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][2]*(SH_MAG[8] - 2.0f*q1*q2)));
if (varInnovMag[1] >= R_MAG) {
faultStatus.bad_ymag = false;
} else {
// the calculation is badly conditioned, so we cannot perform fusion on this step
// we reset the covariance matrix and try again next measurement
CovarianceInit();
obsIndex = 2;
faultStatus.bad_ymag = true;
NavEKF2: use new perf counter API
2015-10-20 02:42:50 -03:00
hal.util->perf_end(_perf_test[2]);
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
return;
}
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
// Z axis
varInnovMag[2] = (P[21][21] + R_MAG + P[16][21]*SH_MAG[5] + P[18][21]*SH_MAG[2] - (2.0f*q0*q1 - 2.0f*q2*q3)*(P[21][17] + P[16][17]*SH_MAG[5] + P[18][17]*SH_MAG[2] - P[0][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][17]*(2.0f*q0*q1 - 2.0f*q2*q3)) - P[0][21]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][21]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + SH_MAG[5]*(P[21][16] + P[16][16]*SH_MAG[5] + P[18][16]*SH_MAG[2] - P[0][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][16]*(2.0f*q0*q1 - 2.0f*q2*q3)) + SH_MAG[2]*(P[21][18] + P[16][18]*SH_MAG[5] + P[18][18]*SH_MAG[2] - P[0][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][18]*(2.0f*q0*q1 - 2.0f*q2*q3)) - (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))*(P[21][0] + P[16][0]*SH_MAG[5] + P[18][0]*SH_MAG[2] - P[0][0]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][0]*(2.0f*q0*q1 - 2.0f*q2*q3)) - P[17][21]*(2.0f*q0*q1 - 2.0f*q2*q3) + (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[21][1] + P[16][1]*SH_MAG[5] + P[18][1]*SH_MAG[2] - P[0][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][1]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][1]*(2.0f*q0*q1 - 2.0f*q2*q3)));
if (varInnovMag[2] >= R_MAG) {
faultStatus.bad_zmag = false;
} else {
// the calculation is badly conditioned, so we cannot perform fusion on this step
// we reset the covariance matrix and try again next measurement
CovarianceInit();
obsIndex = 3;
faultStatus.bad_zmag = true;
hal.util->perf_end(_perf_test[2]);
return;
}
// calculate the innovation test ratios
for (uint8_t i = 0; i<=2; i++) {
Global: rename min and max macros to uppercase The problem with using min() and max() is that they conflict with some C++ headers. Name the macros in uppercase instead. We may go case by case later converting them to be typesafe. Changes generated with: git ls-files '*.cpp' '*.h' -z | xargs -0 sed -i 's/\([^_[:alnum:]]\)max(/\1MAX(/g' git ls-files '*.cpp' '*.h' -z | xargs -0 sed -i 's/\([^_[:alnum:]]\)min(/\1MIN(/g'
2015-11-27 13:11:58 -04:00
magTestRatio[i] = sq(innovMag[i]) / (sq(MAX(0.01f * (float)frontend->_magInnovGate, 1.0f)) * varInnovMag[i]);
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
}
// check the last values from all components and set magnetometer health accordingly
magHealth = (magTestRatio[0] < 1.0f && magTestRatio[1] < 1.0f && magTestRatio[2] < 1.0f);
// if the magnetometer is unhealthy, do not proceed further
if (!magHealth) {
return;
}
for (uint8_t i = 0; i<=stateIndexLim; i++) H_MAG[i] = 0.0f;
H_MAG[1] = SH_MAG[6] - magD*SH_MAG[2] - magN*SH_MAG[5];
H_MAG[2] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2);
H_MAG[16] = SH_MAG[1];
H_MAG[17] = SH_MAG[4];
H_MAG[18] = SH_MAG[7];
H_MAG[19] = 1.0f;
// calculate Kalman gain
SK_MX[0] = 1.0f / varInnovMag[0];
AP_NavEKF2: Explicitly define constants as floats
2015-10-19 05:29:11 -03:00
SK_MX[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2);
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
SK_MX[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
SK_MX[3] = SH_MAG[7];
Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][16]*SH_MAG[1] + P[0][17]*SH_MAG[4] - P[0][1]*SK_MX[2] + P[0][2]*SK_MX[1] + P[0][18]*SK_MX[3]);
Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][16]*SH_MAG[1] + P[1][17]*SH_MAG[4] - P[1][1]*SK_MX[2] + P[1][2]*SK_MX[1] + P[1][18]*SK_MX[3]);
Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][16]*SH_MAG[1] + P[2][17]*SH_MAG[4] - P[2][1]*SK_MX[2] + P[2][2]*SK_MX[1] + P[2][18]*SK_MX[3]);
Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][16]*SH_MAG[1] + P[3][17]*SH_MAG[4] - P[3][1]*SK_MX[2] + P[3][2]*SK_MX[1] + P[3][18]*SK_MX[3]);
Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][16]*SH_MAG[1] + P[4][17]*SH_MAG[4] - P[4][1]*SK_MX[2] + P[4][2]*SK_MX[1] + P[4][18]*SK_MX[3]);
Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][16]*SH_MAG[1] + P[5][17]*SH_MAG[4] - P[5][1]*SK_MX[2] + P[5][2]*SK_MX[1] + P[5][18]*SK_MX[3]);
Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][16]*SH_MAG[1] + P[6][17]*SH_MAG[4] - P[6][1]*SK_MX[2] + P[6][2]*SK_MX[1] + P[6][18]*SK_MX[3]);
Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][16]*SH_MAG[1] + P[7][17]*SH_MAG[4] - P[7][1]*SK_MX[2] + P[7][2]*SK_MX[1] + P[7][18]*SK_MX[3]);
Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][16]*SH_MAG[1] + P[8][17]*SH_MAG[4] - P[8][1]*SK_MX[2] + P[8][2]*SK_MX[1] + P[8][18]*SK_MX[3]);
Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][16]*SH_MAG[1] + P[9][17]*SH_MAG[4] - P[9][1]*SK_MX[2] + P[9][2]*SK_MX[1] + P[9][18]*SK_MX[3]);
Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][16]*SH_MAG[1] + P[10][17]*SH_MAG[4] - P[10][1]*SK_MX[2] + P[10][2]*SK_MX[1] + P[10][18]*SK_MX[3]);
Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][16]*SH_MAG[1] + P[11][17]*SH_MAG[4] - P[11][1]*SK_MX[2] + P[11][2]*SK_MX[1] + P[11][18]*SK_MX[3]);
Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][16]*SH_MAG[1] + P[12][17]*SH_MAG[4] - P[12][1]*SK_MX[2] + P[12][2]*SK_MX[1] + P[12][18]*SK_MX[3]);
Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][16]*SH_MAG[1] + P[13][17]*SH_MAG[4] - P[13][1]*SK_MX[2] + P[13][2]*SK_MX[1] + P[13][18]*SK_MX[3]);
Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][16]*SH_MAG[1] + P[14][17]*SH_MAG[4] - P[14][1]*SK_MX[2] + P[14][2]*SK_MX[1] + P[14][18]*SK_MX[3]);
AP_NavEKF2: Miscellaneous tuning changes
2015-10-15 07:17:07 -03:00
Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][16]*SH_MAG[1] + P[15][17]*SH_MAG[4] - P[15][1]*SK_MX[2] + P[15][2]*SK_MX[1] + P[15][18]*SK_MX[3]);
AP_NavEKF2: enable fine grained perf tuning for mag fusion
2015-10-19 18:32:07 -03:00
// end perf block
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
// zero Kalman gains to inhibit wind state estimation
if (!inhibitWindStates) {
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][16]*SH_MAG[1] + P[22][17]*SH_MAG[4] - P[22][1]*SK_MX[2] + P[22][2]*SK_MX[1] + P[22][18]*SK_MX[3]);
Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][16]*SH_MAG[1] + P[23][17]*SH_MAG[4] - P[23][1]*SK_MX[2] + P[23][2]*SK_MX[1] + P[23][18]*SK_MX[3]);
} else {
Kfusion[22] = 0.0f;
Kfusion[23] = 0.0f;
}
// zero Kalman gains to inhibit magnetic field state estimation
if (!inhibitMagStates) {
Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][16]*SH_MAG[1] + P[16][17]*SH_MAG[4] - P[16][1]*SK_MX[2] + P[16][2]*SK_MX[1] + P[16][18]*SK_MX[3]);
Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][16]*SH_MAG[1] + P[17][17]*SH_MAG[4] - P[17][1]*SK_MX[2] + P[17][2]*SK_MX[1] + P[17][18]*SK_MX[3]);
Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][16]*SH_MAG[1] + P[18][17]*SH_MAG[4] - P[18][1]*SK_MX[2] + P[18][2]*SK_MX[1] + P[18][18]*SK_MX[3]);
Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][16]*SH_MAG[1] + P[19][17]*SH_MAG[4] - P[19][1]*SK_MX[2] + P[19][2]*SK_MX[1] + P[19][18]*SK_MX[3]);
Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][16]*SH_MAG[1] + P[20][17]*SH_MAG[4] - P[20][1]*SK_MX[2] + P[20][2]*SK_MX[1] + P[20][18]*SK_MX[3]);
Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][16]*SH_MAG[1] + P[21][17]*SH_MAG[4] - P[21][1]*SK_MX[2] + P[21][2]*SK_MX[1] + P[21][18]*SK_MX[3]);
} else {
for (uint8_t i=16; i<=21; i++) {
Kfusion[i] = 0.0f;
}
}
// reset the observation index to 0 (we start by fusing the X measurement)
obsIndex = 0;
// set flags to indicate to other processes that fusion has been performed and is required on the next frame
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
magFusePerformed = true;
magFuseRequired = true;
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
NavEKF2: use new perf counter API
2015-10-20 02:42:50 -03:00
hal.util->perf_end(_perf_test[2]);
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
}
else if (obsIndex == 1) // we are now fusing the Y measurement
{
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
NavEKF2: use new perf counter API
2015-10-20 02:42:50 -03:00
hal.util->perf_begin(_perf_test[3]);
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
// calculate observation jacobians
for (uint8_t i = 0; i<=stateIndexLim; i++) H_MAG[i] = 0.0f;
H_MAG[0] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
H_MAG[2] = - magE*SH_MAG[4] - magD*SH_MAG[7] - magN*SH_MAG[1];
AP_NavEKF2: Explicitly define constants as floats
2015-10-19 05:29:11 -03:00
H_MAG[16] = 2.0f*q1*q2 - SH_MAG[8];
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
H_MAG[17] = SH_MAG[0];
H_MAG[18] = SH_MAG[3];
AP_NavEKF2: Explicitly define constants as floats
2015-10-19 05:29:11 -03:00
H_MAG[20] = 1.0f;
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
// calculate Kalman gain
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
SK_MY[0] = 1.0f / varInnovMag[1];
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
SK_MY[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
SK_MY[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
AP_NavEKF2: Explicitly define constants as floats
2015-10-19 05:29:11 -03:00
SK_MY[3] = SH_MAG[8] - 2.0f*q1*q2;
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][17]*SH_MAG[0] + P[0][18]*SH_MAG[3] + P[0][0]*SK_MY[2] - P[0][2]*SK_MY[1] - P[0][16]*SK_MY[3]);
Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][17]*SH_MAG[0] + P[1][18]*SH_MAG[3] + P[1][0]*SK_MY[2] - P[1][2]*SK_MY[1] - P[1][16]*SK_MY[3]);
Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][17]*SH_MAG[0] + P[2][18]*SH_MAG[3] + P[2][0]*SK_MY[2] - P[2][2]*SK_MY[1] - P[2][16]*SK_MY[3]);
Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][17]*SH_MAG[0] + P[3][18]*SH_MAG[3] + P[3][0]*SK_MY[2] - P[3][2]*SK_MY[1] - P[3][16]*SK_MY[3]);
Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][17]*SH_MAG[0] + P[4][18]*SH_MAG[3] + P[4][0]*SK_MY[2] - P[4][2]*SK_MY[1] - P[4][16]*SK_MY[3]);
Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][17]*SH_MAG[0] + P[5][18]*SH_MAG[3] + P[5][0]*SK_MY[2] - P[5][2]*SK_MY[1] - P[5][16]*SK_MY[3]);
Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][17]*SH_MAG[0] + P[6][18]*SH_MAG[3] + P[6][0]*SK_MY[2] - P[6][2]*SK_MY[1] - P[6][16]*SK_MY[3]);
Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][17]*SH_MAG[0] + P[7][18]*SH_MAG[3] + P[7][0]*SK_MY[2] - P[7][2]*SK_MY[1] - P[7][16]*SK_MY[3]);
Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][17]*SH_MAG[0] + P[8][18]*SH_MAG[3] + P[8][0]*SK_MY[2] - P[8][2]*SK_MY[1] - P[8][16]*SK_MY[3]);
Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][17]*SH_MAG[0] + P[9][18]*SH_MAG[3] + P[9][0]*SK_MY[2] - P[9][2]*SK_MY[1] - P[9][16]*SK_MY[3]);
Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][17]*SH_MAG[0] + P[10][18]*SH_MAG[3] + P[10][0]*SK_MY[2] - P[10][2]*SK_MY[1] - P[10][16]*SK_MY[3]);
Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][17]*SH_MAG[0] + P[11][18]*SH_MAG[3] + P[11][0]*SK_MY[2] - P[11][2]*SK_MY[1] - P[11][16]*SK_MY[3]);
Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][17]*SH_MAG[0] + P[12][18]*SH_MAG[3] + P[12][0]*SK_MY[2] - P[12][2]*SK_MY[1] - P[12][16]*SK_MY[3]);
Kfusion[13] = SK_MY[0]*(P[13][20] + P[13][17]*SH_MAG[0] + P[13][18]*SH_MAG[3] + P[13][0]*SK_MY[2] - P[13][2]*SK_MY[1] - P[13][16]*SK_MY[3]);
Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][17]*SH_MAG[0] + P[14][18]*SH_MAG[3] + P[14][0]*SK_MY[2] - P[14][2]*SK_MY[1] - P[14][16]*SK_MY[3]);
AP_NavEKF2: Miscellaneous tuning changes
2015-10-15 07:17:07 -03:00
Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][17]*SH_MAG[0] + P[15][18]*SH_MAG[3] + P[15][0]*SK_MY[2] - P[15][2]*SK_MY[1] - P[15][16]*SK_MY[3]);
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
// zero Kalman gains to inhibit wind state estimation
if (!inhibitWindStates) {
Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][17]*SH_MAG[0] + P[22][18]*SH_MAG[3] + P[22][0]*SK_MY[2] - P[22][2]*SK_MY[1] - P[22][16]*SK_MY[3]);
Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][17]*SH_MAG[0] + P[23][18]*SH_MAG[3] + P[23][0]*SK_MY[2] - P[23][2]*SK_MY[1] - P[23][16]*SK_MY[3]);
} else {
Kfusion[22] = 0.0f;
Kfusion[23] = 0.0f;
}
// zero Kalman gains to inhibit magnetic field state estimation
if (!inhibitMagStates) {
Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][17]*SH_MAG[0] + P[16][18]*SH_MAG[3] + P[16][0]*SK_MY[2] - P[16][2]*SK_MY[1] - P[16][16]*SK_MY[3]);
Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][17]*SH_MAG[0] + P[17][18]*SH_MAG[3] + P[17][0]*SK_MY[2] - P[17][2]*SK_MY[1] - P[17][16]*SK_MY[3]);
Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][17]*SH_MAG[0] + P[18][18]*SH_MAG[3] + P[18][0]*SK_MY[2] - P[18][2]*SK_MY[1] - P[18][16]*SK_MY[3]);
Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][17]*SH_MAG[0] + P[19][18]*SH_MAG[3] + P[19][0]*SK_MY[2] - P[19][2]*SK_MY[1] - P[19][16]*SK_MY[3]);
Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][17]*SH_MAG[0] + P[20][18]*SH_MAG[3] + P[20][0]*SK_MY[2] - P[20][2]*SK_MY[1] - P[20][16]*SK_MY[3]);
Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][17]*SH_MAG[0] + P[21][18]*SH_MAG[3] + P[21][0]*SK_MY[2] - P[21][2]*SK_MY[1] - P[21][16]*SK_MY[3]);
} else {
for (uint8_t i=16; i<=21; i++) {
Kfusion[i] = 0.0f;
}
}
// set flags to indicate to other processes that fusion has been performede and is required on the next frame
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
magFusePerformed = true;
magFuseRequired = true;
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
NavEKF2: use new perf counter API
2015-10-20 02:42:50 -03:00
hal.util->perf_end(_perf_test[3]);
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
}
else if (obsIndex == 2) // we are now fusing the Z measurement
{
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
NavEKF2: use new perf counter API
2015-10-20 02:42:50 -03:00
hal.util->perf_begin(_perf_test[4]);
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
// calculate observation jacobians
for (uint8_t i = 0; i<=stateIndexLim; i++) H_MAG[i] = 0.0f;
AP_NavEKF2: Explicitly define constants as floats
2015-10-19 05:29:11 -03:00
H_MAG[0] = magN*(SH_MAG[8] - 2.0f*q1*q2) - magD*SH_MAG[3] - magE*SH_MAG[0];
AP_NavEKF2: Improve efficiency of Z magnetometer fusion
2015-10-16 07:03:08 -03:00
H_MAG[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
H_MAG[16] = SH_MAG[5];
AP_NavEKF2: Explicitly define constants as floats
2015-10-19 05:29:11 -03:00
H_MAG[17] = 2.0f*q2*q3 - 2.0f*q0*q1;
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
H_MAG[18] = SH_MAG[2];
AP_NavEKF2: Explicitly define constants as floats
2015-10-19 05:29:11 -03:00
H_MAG[21] = 1.0f;
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
// calculate Kalman gain
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
SK_MZ[0] = 1.0f / varInnovMag[2];
AP_NavEKF2: Explicitly define constants as floats
2015-10-19 05:29:11 -03:00
SK_MZ[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2);
AP_NavEKF2: Improve efficiency of Z magnetometer fusion
2015-10-16 07:03:08 -03:00
SK_MZ[2] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
AP_NavEKF2: Explicitly define constants as floats
2015-10-19 05:29:11 -03:00
SK_MZ[3] = 2.0f*q0*q1 - 2.0f*q2*q3;
AP_NavEKF2: Improve efficiency of Z magnetometer fusion
2015-10-16 07:03:08 -03:00
Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][18]*SH_MAG[2] + P[0][16]*SH_MAG[5] - P[0][0]*SK_MZ[1] + P[0][1]*SK_MZ[2] - P[0][17]*SK_MZ[3]);
Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][18]*SH_MAG[2] + P[1][16]*SH_MAG[5] - P[1][0]*SK_MZ[1] + P[1][1]*SK_MZ[2] - P[1][17]*SK_MZ[3]);
Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][18]*SH_MAG[2] + P[2][16]*SH_MAG[5] - P[2][0]*SK_MZ[1] + P[2][1]*SK_MZ[2] - P[2][17]*SK_MZ[3]);
Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][18]*SH_MAG[2] + P[3][16]*SH_MAG[5] - P[3][0]*SK_MZ[1] + P[3][1]*SK_MZ[2] - P[3][17]*SK_MZ[3]);
Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][18]*SH_MAG[2] + P[4][16]*SH_MAG[5] - P[4][0]*SK_MZ[1] + P[4][1]*SK_MZ[2] - P[4][17]*SK_MZ[3]);
Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][18]*SH_MAG[2] + P[5][16]*SH_MAG[5] - P[5][0]*SK_MZ[1] + P[5][1]*SK_MZ[2] - P[5][17]*SK_MZ[3]);
Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][18]*SH_MAG[2] + P[6][16]*SH_MAG[5] - P[6][0]*SK_MZ[1] + P[6][1]*SK_MZ[2] - P[6][17]*SK_MZ[3]);
Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][18]*SH_MAG[2] + P[7][16]*SH_MAG[5] - P[7][0]*SK_MZ[1] + P[7][1]*SK_MZ[2] - P[7][17]*SK_MZ[3]);
Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][18]*SH_MAG[2] + P[8][16]*SH_MAG[5] - P[8][0]*SK_MZ[1] + P[8][1]*SK_MZ[2] - P[8][17]*SK_MZ[3]);
Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][18]*SH_MAG[2] + P[9][16]*SH_MAG[5] - P[9][0]*SK_MZ[1] + P[9][1]*SK_MZ[2] - P[9][17]*SK_MZ[3]);
Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][18]*SH_MAG[2] + P[10][16]*SH_MAG[5] - P[10][0]*SK_MZ[1] + P[10][1]*SK_MZ[2] - P[10][17]*SK_MZ[3]);
Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][18]*SH_MAG[2] + P[11][16]*SH_MAG[5] - P[11][0]*SK_MZ[1] + P[11][1]*SK_MZ[2] - P[11][17]*SK_MZ[3]);
Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][18]*SH_MAG[2] + P[12][16]*SH_MAG[5] - P[12][0]*SK_MZ[1] + P[12][1]*SK_MZ[2] - P[12][17]*SK_MZ[3]);
Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][18]*SH_MAG[2] + P[13][16]*SH_MAG[5] - P[13][0]*SK_MZ[1] + P[13][1]*SK_MZ[2] - P[13][17]*SK_MZ[3]);
Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][18]*SH_MAG[2] + P[14][16]*SH_MAG[5] - P[14][0]*SK_MZ[1] + P[14][1]*SK_MZ[2] - P[14][17]*SK_MZ[3]);
Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][18]*SH_MAG[2] + P[15][16]*SH_MAG[5] - P[15][0]*SK_MZ[1] + P[15][1]*SK_MZ[2] - P[15][17]*SK_MZ[3]);
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
// zero Kalman gains to inhibit wind state estimation
if (!inhibitWindStates) {
AP_NavEKF2: Improve efficiency of Z magnetometer fusion
2015-10-16 07:03:08 -03:00
Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][18]*SH_MAG[2] + P[22][16]*SH_MAG[5] - P[22][0]*SK_MZ[1] + P[22][1]*SK_MZ[2] - P[22][17]*SK_MZ[3]);
Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][18]*SH_MAG[2] + P[23][16]*SH_MAG[5] - P[23][0]*SK_MZ[1] + P[23][1]*SK_MZ[2] - P[23][17]*SK_MZ[3]);
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
} else {
Kfusion[22] = 0.0f;
Kfusion[23] = 0.0f;
}
// zero Kalman gains to inhibit magnetic field state estimation
if (!inhibitMagStates) {
AP_NavEKF2: Improve efficiency of Z magnetometer fusion
2015-10-16 07:03:08 -03:00
Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][18]*SH_MAG[2] + P[16][16]*SH_MAG[5] - P[16][0]*SK_MZ[1] + P[16][1]*SK_MZ[2] - P[16][17]*SK_MZ[3]);
Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][18]*SH_MAG[2] + P[17][16]*SH_MAG[5] - P[17][0]*SK_MZ[1] + P[17][1]*SK_MZ[2] - P[17][17]*SK_MZ[3]);
Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][18]*SH_MAG[2] + P[18][16]*SH_MAG[5] - P[18][0]*SK_MZ[1] + P[18][1]*SK_MZ[2] - P[18][17]*SK_MZ[3]);
Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][18]*SH_MAG[2] + P[19][16]*SH_MAG[5] - P[19][0]*SK_MZ[1] + P[19][1]*SK_MZ[2] - P[19][17]*SK_MZ[3]);
Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][18]*SH_MAG[2] + P[20][16]*SH_MAG[5] - P[20][0]*SK_MZ[1] + P[20][1]*SK_MZ[2] - P[20][17]*SK_MZ[3]);
Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][18]*SH_MAG[2] + P[21][16]*SH_MAG[5] - P[21][0]*SK_MZ[1] + P[21][1]*SK_MZ[2] - P[21][17]*SK_MZ[3]);
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
} else {
for (uint8_t i=16; i<=21; i++) {
Kfusion[i] = 0.0f;
}
}
// set flags to indicate to other processes that fusion has been performede and is required on the next frame
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
magFusePerformed = true;
magFuseRequired = false;
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
NavEKF2: use new perf counter API
2015-10-20 02:42:50 -03:00
hal.util->perf_end(_perf_test[4]);
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
}
AP_NavEKF2: enable fine grained perf tuning for mag fusion
2015-10-19 18:32:07 -03:00
NavEKF2: use new perf counter API
2015-10-20 02:42:50 -03:00
hal.util->perf_begin(_perf_test[5]);
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
// correct the covariance P = (I - K*H)*P
// take advantage of the empty columns in KH to reduce the
// number of operations
for (unsigned i = 0; i<=stateIndexLim; i++) {
for (unsigned j = 0; j<=2; j++) {
KH[i][j] = Kfusion[i] * H_MAG[j];
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
}
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
for (unsigned j = 3; j<=15; j++) {
KH[i][j] = 0.0f;
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
}
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
for (unsigned j = 16; j<=21; j++) {
KH[i][j] = Kfusion[i] * H_MAG[j];
}
for (unsigned j = 22; j<=23; j++) {
KH[i][j] = 0.0f;
}
}
for (unsigned j = 0; j<=stateIndexLim; j++) {
AP_NavEKF2: down to 218us for test[9]
2015-10-19 18:57:39 -03:00
for (unsigned i = 0; i<=stateIndexLim; i++) {
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
ftype res = 0;
res += KH[i][0] * P[0][j];
res += KH[i][1] * P[1][j];
res += KH[i][2] * P[2][j];
res += KH[i][16] * P[16][j];
res += KH[i][17] * P[17][j];
res += KH[i][18] * P[18][j];
res += KH[i][19] * P[19][j];
res += KH[i][20] * P[20][j];
res += KH[i][21] * P[21][j];
KHP[i][j] = res;
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
}
}
AP_NavEKF2: add error checking and isolation to 3D amg fusion
2016-05-10 04:04:31 -03:00
// Check that we are not going to drive any variances negative and skip the update if so
bool healthyFusion = true;
for (uint8_t i= 0; i<=stateIndexLim; i++) {
if (KHP[i][i] > P[i][i]) {
healthyFusion = false;
}
}
if (healthyFusion) {
// update the covariance matrix
for (uint8_t i= 0; i<=stateIndexLim; i++) {
for (uint8_t j= 0; j<=stateIndexLim; j++) {
P[i][j] = P[i][j] - KHP[i][j];
}
}
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning.
ForceSymmetry();
ConstrainVariances();
// update the states
// zero the attitude error state - by definition it is assumed to be zero before each observaton fusion
stateStruct.angErr.zero();
// correct the state vector
for (uint8_t j= 0; j<=stateIndexLim; j++) {
statesArray[j] = statesArray[j] - Kfusion[j] * innovMag[obsIndex];
}
// the first 3 states represent the angular misalignment vector. This is
// is used to correct the estimated quaternion on the current time step
stateStruct.quat.rotate(stateStruct.angErr);
} else {
// record bad axis
if (obsIndex == 0) {
faultStatus.bad_xmag = true;
} else if (obsIndex == 1) {
faultStatus.bad_ymag = true;
} else if (obsIndex == 2) {
faultStatus.bad_zmag = true;
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
}
}
AP_NavEKF2: fix bug in performance timer
2015-11-17 19:25:28 -04:00
hal.util->perf_end(_perf_test[5]);
AP_NavEKF2: Require all magnetometer axes to pass innovation test The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis. This was not achieved with the previous implementation. These changes move the innovation consistency checks for all three axes to the top before any axes are fused. Unnecessary performance timers have been removed.
2015-11-08 05:36:41 -04:00
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
}
AP_NavEKF2: Update function header comments
2015-10-17 20:32:11 -03:00
/*
AP_NavEKF2: Fix bug in magnetic heading and declination fusion equations. The derivation incorrectly used a tan instead of an atan function. This applies the corrected auto-code.
2016-02-16 01:41:48 -04:00
* Fuse magnetic heading measurement using explicit algebraic equations generated with Matlab symbolic toolbox.
AP_NavEKF2: Update function header comments
2015-10-17 20:32:11 -03:00
* The script file used to generate these and other equations in this filter can be found here:
* https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m
AP_NavEKF2: Update magnetometer fusion documentation
2015-10-18 19:37:47 -03:00
* This fusion method only modifies the orientation, does not require use of the magnetic field states and is computatonally cheaper.
* It is suitable for use when the external magnetic field environment is disturbed (eg close to metal structures, on ground).
* It is not as robust to magneometer failures.
AP_NavEKF2: Fix bug in magnetic heading and declination fusion equations. The derivation incorrectly used a tan instead of an atan function. This applies the corrected auto-code.
2016-02-16 01:41:48 -04:00
* It is not suitable for operation where the horizontal magnetic field strength is weak (within 30 degreees latitude of the the magnetic poles)
AP_NavEKF2: Update function header comments
2015-10-17 20:32:11 -03:00
*/
AP_NavEKF2: Improved magnetic heading fusion Use an Euler yaw heading that switches between a 321 and 312 rotation sequence to avoid areas of singularity. Using Euler yaw decouples the observation from the roll and pitch states and prevents magnetic disturbances from affecting roll and pitch via the magnetometer fusion process.
2016-02-29 18:51:45 -04:00
void NavEKF2_core::fuseEulerYaw()
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
{
float q0 = stateStruct.quat[0];
float q1 = stateStruct.quat[1];
float q2 = stateStruct.quat[2];
float q3 = stateStruct.quat[3];
// compass measurement error variance (rad^2)
AP_NavEKF2: Add tuning parameters for magentic yaw fusion Enable simple magnetic yaw fusion observation noise and innovation consistency check gate width to be tuned independantly.
2016-06-13 07:51:45 -03:00
const float R_YAW = sq(frontend->_yawNoise);
AP_NavEKF2: Improved magnetic heading fusion Use an Euler yaw heading that switches between a 321 and 312 rotation sequence to avoid areas of singularity. Using Euler yaw decouples the observation from the roll and pitch states and prevents magnetic disturbances from affecting roll and pitch via the magnetometer fusion process.
2016-02-29 18:51:45 -04:00
// calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix
// determine if a 321 or 312 Euler sequence is best
float predicted_yaw;
float H_YAW[3];
Matrix3f Tbn_zeroYaw;
if (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) {
// calculate observation jacobian when we are observing the first rotation in a 321 sequence
float t2 = q0*q0;
float t3 = q1*q1;
float t4 = q2*q2;
float t5 = q3*q3;
float t6 = t2+t3-t4-t5;
float t7 = q0*q3*2.0f;
float t8 = q1*q2*2.0f;
float t9 = t7+t8;
AP_NavEKF: Protect against possible div by 0
2016-03-01 02:51:04 -04:00
float t10 = sq(t6);
if (t10 > 1e-6f) {
t10 = 1.0f / t10;
} else {
return;
}
AP_NavEKF2: Improved magnetic heading fusion Use an Euler yaw heading that switches between a 321 and 312 rotation sequence to avoid areas of singularity. Using Euler yaw decouples the observation from the roll and pitch states and prevents magnetic disturbances from affecting roll and pitch via the magnetometer fusion process.
2016-02-29 18:51:45 -04:00
float t11 = t9*t9;
float t12 = t10*t11;
float t13 = t12+1.0f;
float t14;
if (fabsf(t13) > 1e-3f) {
t14 = 1.0f/t13;
} else {
return;
}
AP_NavEKF: Protect against possible div by 0
2016-03-01 02:51:04 -04:00
float t15 = 1.0f/t6;
AP_NavEKF2: Improved magnetic heading fusion Use an Euler yaw heading that switches between a 321 and 312 rotation sequence to avoid areas of singularity. Using Euler yaw decouples the observation from the roll and pitch states and prevents magnetic disturbances from affecting roll and pitch via the magnetometer fusion process.
2016-02-29 18:51:45 -04:00
H_YAW[0] = 0.0f;
H_YAW[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f));
H_YAW[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8));
// Get the 321 euler angles
Vector3f euler321;
stateStruct.quat.to_euler(euler321.x, euler321.y, euler321.z);
predicted_yaw = euler321.z;
// set the yaw to zero and calculate the zero yaw rotation from body to earth frame
Tbn_zeroYaw.from_euler(euler321.x, euler321.y, 0.0f);
} else {
// calculate observaton jacobian when we are observing a rotation in a 312 sequence
float t2 = q0*q0;
float t3 = q1*q1;
float t4 = q2*q2;
float t5 = q3*q3;
float t6 = t2-t3+t4-t5;
float t7 = q0*q3*2.0f;
float t10 = q1*q2*2.0f;
float t8 = t7-t10;
AP_NavEKF: Protect against possible div by 0
2016-03-01 02:51:04 -04:00
float t9 = sq(t6);
if (t9 > 1e-6f) {
t9 = 1.0f/t9;
} else {
return;
}
AP_NavEKF2: Improved magnetic heading fusion Use an Euler yaw heading that switches between a 321 and 312 rotation sequence to avoid areas of singularity. Using Euler yaw decouples the observation from the roll and pitch states and prevents magnetic disturbances from affecting roll and pitch via the magnetometer fusion process.
2016-02-29 18:51:45 -04:00
float t11 = t8*t8;
float t12 = t9*t11;
float t13 = t12+1.0f;
float t14;
if (fabsf(t13) > 1e-3f) {
t14 = 1.0f/t13;
} else {
return;
}
AP_NavEKF: Protect against possible div by 0
2016-03-01 02:51:04 -04:00
float t15 = 1.0f/t6;
AP_NavEKF2: Improved magnetic heading fusion Use an Euler yaw heading that switches between a 321 and 312 rotation sequence to avoid areas of singularity. Using Euler yaw decouples the observation from the roll and pitch states and prevents magnetic disturbances from affecting roll and pitch via the magnetometer fusion process.
2016-02-29 18:51:45 -04:00
H_YAW[0] = -t14*(t15*(q0*q2*2.0+q1*q3*2.0)-t8*t9*(q0*q1*2.0-q2*q3*2.0));
H_YAW[1] = 0.0f;
H_YAW[2] = t14*(t15*(t2+t3-t4-t5)+t8*t9*(t7+t10));
// Get the 321 euler angles
Vector3f euler312 = stateStruct.quat.to_vector312();
predicted_yaw = euler312.z;
// set the yaw to zero and calculate the zero yaw rotation from body to earth frame
Tbn_zeroYaw.from_euler312(euler312.x, euler312.y, 0.0f);
AP_NavEKF2: Fix bug in magnetic heading and declination fusion equations. The derivation incorrectly used a tan instead of an atan function. This applies the corrected auto-code.
2016-02-16 01:41:48 -04:00
}
AP_NavEKF2: Eliminate simple compass fusion singularities near +-90 deg pitch The use of yaw angle fusion during startup and ground operation causes problems with tail-sitter vehicle types. Instead of observing an Euler yaw angle, we now observe the yaw angle obtained by projecting the measured magnetic field onto the the horizontal plain. This avoids the singularities associated with the observation of Euler yaw angle.
2016-02-20 05:37:17 -04:00
AP_NavEKF2: Improved magnetic heading fusion Use an Euler yaw heading that switches between a 321 and 312 rotation sequence to avoid areas of singularity. Using Euler yaw decouples the observation from the roll and pitch states and prevents magnetic disturbances from affecting roll and pitch via the magnetometer fusion process.
2016-02-29 18:51:45 -04:00
// rotate measured mag components into earth frame
Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag;
// Use the difference between the horizontal projection and declination to give the measured yaw
AP_NavEKF2: Allow use in planes without a magnetometer Implements the following techniques to enable planes to operate without magnetometers. 1) When on ground with mag use inhibited, a synthetic heading equal to current heading is fused to prevent uncontrolled covariance growth. 2) When transitioning to in-flight, the delta between inertial and GPS velocity vector is used to align the yaw. 3) The yaw gyro bias state variance is reset following an in-flight heading reset to enable the yaw gyro bias to be learned faster.
2016-04-08 02:35:21 -03:00
// If we can't use compass data, set the meaurement to the predicted
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
// to prevent uncontrolled variance growth whilst on ground without magnetometer
AP_NavEKF2: Allow use in planes without a magnetometer Implements the following techniques to enable planes to operate without magnetometers. 1) When on ground with mag use inhibited, a synthetic heading equal to current heading is fused to prevent uncontrolled covariance growth. 2) When transitioning to in-flight, the delta between inertial and GPS velocity vector is used to align the yaw. 3) The yaw gyro bias state variance is reset following an in-flight heading reset to enable the yaw gyro bias to be learned faster.
2016-04-08 02:35:21 -03:00
float measured_yaw;
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
if (use_compass() && yawAlignComplete && magStateInitComplete) {
AP_NavEKF2: Allow use in planes without a magnetometer Implements the following techniques to enable planes to operate without magnetometers. 1) When on ground with mag use inhibited, a synthetic heading equal to current heading is fused to prevent uncontrolled covariance growth. 2) When transitioning to in-flight, the delta between inertial and GPS velocity vector is used to align the yaw. 3) The yaw gyro bias state variance is reset following an in-flight heading reset to enable the yaw gyro bias to be learned faster.
2016-04-08 02:35:21 -03:00
measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + _ahrs->get_compass()->get_declination());
} else {
measured_yaw = predicted_yaw;
}
AP_NavEKF2: Improved magnetic heading fusion Use an Euler yaw heading that switches between a 321 and 312 rotation sequence to avoid areas of singularity. Using Euler yaw decouples the observation from the roll and pitch states and prevents magnetic disturbances from affecting roll and pitch via the magnetometer fusion process.
2016-02-29 18:51:45 -04:00
// Calculate the innovation
float innovation = wrap_PI(predicted_yaw - measured_yaw);
// Copy raw value to output variable used for data logging
innovYaw = innovation;
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero
float PH[3];
AP_NavEKF2: Improved magnetic heading fusion Use an Euler yaw heading that switches between a 321 and 312 rotation sequence to avoid areas of singularity. Using Euler yaw decouples the observation from the roll and pitch states and prevents magnetic disturbances from affecting roll and pitch via the magnetometer fusion process.
2016-02-29 18:51:45 -04:00
float varInnov = R_YAW;
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
for (uint8_t rowIndex=0; rowIndex<=2; rowIndex++) {
PH[rowIndex] = 0.0f;
for (uint8_t colIndex=0; colIndex<=2; colIndex++) {
AP_NavEKF2: Improved magnetic heading fusion Use an Euler yaw heading that switches between a 321 and 312 rotation sequence to avoid areas of singularity. Using Euler yaw decouples the observation from the roll and pitch states and prevents magnetic disturbances from affecting roll and pitch via the magnetometer fusion process.
2016-02-29 18:51:45 -04:00
PH[rowIndex] += P[rowIndex][colIndex]*H_YAW[colIndex];
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
}
AP_NavEKF2: Improved magnetic heading fusion Use an Euler yaw heading that switches between a 321 and 312 rotation sequence to avoid areas of singularity. Using Euler yaw decouples the observation from the roll and pitch states and prevents magnetic disturbances from affecting roll and pitch via the magnetometer fusion process.
2016-02-29 18:51:45 -04:00
varInnov += H_YAW[rowIndex]*PH[rowIndex];
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
}
AP_NavEKF2: Remove potential for division by zero
2015-10-30 03:07:38 -03:00
float varInnovInv;
AP_NavEKF2: Improved magnetic heading fusion Use an Euler yaw heading that switches between a 321 and 312 rotation sequence to avoid areas of singularity. Using Euler yaw decouples the observation from the roll and pitch states and prevents magnetic disturbances from affecting roll and pitch via the magnetometer fusion process.
2016-02-29 18:51:45 -04:00
if (varInnov >= R_YAW) {
AP_NavEKF2: Remove potential for division by zero
2015-10-30 03:07:38 -03:00
varInnovInv = 1.0f / varInnov;
AP_NavEKF2: fix yaw fusion numerical health reporting
2016-05-10 04:47:31 -03:00
// output numerical health status
faultStatus.bad_yaw = false;
AP_NavEKF2: Remove potential for division by zero
2015-10-30 03:07:38 -03:00
} else {
// the calculation is badly conditioned, so we cannot perform fusion on this step
// we reset the covariance matrix and try again next measurement
CovarianceInit();
AP_NavEKF2: fix yaw fusion numerical health reporting
2016-05-10 04:47:31 -03:00
// output numerical health status
faultStatus.bad_yaw = true;
AP_NavEKF2: Remove potential for division by zero
2015-10-30 03:07:38 -03:00
return;
}
AP_NavEKF2: fix yaw fusion numerical health reporting
2016-05-10 04:47:31 -03:00
// calculate Kalman gain
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
for (uint8_t rowIndex=0; rowIndex<=stateIndexLim; rowIndex++) {
Kfusion[rowIndex] = 0.0f;
for (uint8_t colIndex=0; colIndex<=2; colIndex++) {
AP_NavEKF2: Improved magnetic heading fusion Use an Euler yaw heading that switches between a 321 and 312 rotation sequence to avoid areas of singularity. Using Euler yaw decouples the observation from the roll and pitch states and prevents magnetic disturbances from affecting roll and pitch via the magnetometer fusion process.
2016-02-29 18:51:45 -04:00
Kfusion[rowIndex] += P[rowIndex][colIndex]*H_YAW[colIndex];
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
}
Kfusion[rowIndex] *= varInnovInv;
}
AP_NavEKF2: Add improved health monitoring when using simple compass yaw fusion
2015-10-28 22:36:53 -03:00
// calculate the innovation test ratio
AP_NavEKF2: Add tuning parameters for magentic yaw fusion Enable simple magnetic yaw fusion observation noise and innovation consistency check gate width to be tuned independantly.
2016-06-13 07:51:45 -03:00
yawTestRatio = sq(innovation) / (sq(MAX(0.01f * (float)frontend->_yawInnovGate, 1.0f)) * varInnov);
AP_NavEKF2: Add improved health monitoring when using simple compass yaw fusion
2015-10-28 22:36:53 -03:00
// Declare the magnetometer unhealthy if the innovation test fails
if (yawTestRatio > 1.0f) {
magHealth = false;
// On the ground a large innovation could be due to large initial gyro bias or magnetic interference from nearby objects
// If we are flying, then it is more likely due to a magnetometer fault and we should not fuse the data
if (inFlight) {
return;
}
} else {
magHealth = true;
}
AP_NavEKF: Limit heading innovations after the consistency check
2016-02-16 03:42:59 -04:00
// limit the innovation so that initial corrections are not too large
if (innovation > 0.5f) {
innovation = 0.5f;
} else if (innovation < -0.5f) {
innovation = -0.5f;
}
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
// correct the covariance using P = P - K*H*P taking advantage of the fact that only the first 3 elements in H are non zero
AP_NavEKF2: add error checking and isolation to mag heading fusion
2016-05-10 04:25:09 -03:00
// calculate K*H*P
for (uint8_t row = 0; row <= stateIndexLim; row++) {
for (uint8_t column = 0; column <= 2; column++) {
KH[row][column] = Kfusion[row] * H_YAW[column];
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
}
}
AP_NavEKF2: add error checking and isolation to mag heading fusion
2016-05-10 04:25:09 -03:00
for (uint8_t row = 0; row <= stateIndexLim; row++) {
for (uint8_t column = 0; column <= stateIndexLim; column++) {
float tmp = KH[row][0] * P[0][column];
tmp += KH[row][1] * P[1][column];
tmp += KH[row][2] * P[2][column];
KHP[row][column] = tmp;
}
}
// Check that we are not going to drive any variances negative and skip the update if so
bool healthyFusion = true;
for (uint8_t i= 0; i<=stateIndexLim; i++) {
if (KHP[i][i] > P[i][i]) {
healthyFusion = false;
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
}
}
AP_NavEKF2: add error checking and isolation to mag heading fusion
2016-05-10 04:25:09 -03:00
if (healthyFusion) {
// update the covariance matrix
for (uint8_t i= 0; i<=stateIndexLim; i++) {
for (uint8_t j= 0; j<=stateIndexLim; j++) {
P[i][j] = P[i][j] - KHP[i][j];
}
}
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning.
ForceSymmetry();
ConstrainVariances();
// zero the attitude error state - by definition it is assumed to be zero before each observaton fusion
stateStruct.angErr.zero();
// correct the state vector
for (uint8_t i=0; i<=stateIndexLim; i++) {
statesArray[i] -= Kfusion[i] * innovation;
}
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
AP_NavEKF2: add error checking and isolation to mag heading fusion
2016-05-10 04:25:09 -03:00
// the first 3 states represent the angular misalignment vector. This is
// is used to correct the estimated quaternion on the current time step
stateStruct.quat.rotate(stateStruct.angErr);
AP_NavEKF2: fix yaw fusion numerical health reporting
2016-05-10 04:47:31 -03:00
// record fusion numerical health status
AP_NavEKF2: add error checking and isolation to mag heading fusion
2016-05-10 04:25:09 -03:00
faultStatus.bad_yaw = false;
} else {
AP_NavEKF2: fix yaw fusion numerical health reporting
2016-05-10 04:47:31 -03:00
// record fusion numerical health status
AP_NavEKF2: add error checking and isolation to mag heading fusion
2016-05-10 04:25:09 -03:00
faultStatus.bad_yaw = true;
}
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
}
AP_NavEKF2: Allow use of magnetometer learning during optical flow nav Adds fusion of the declination when there are no earth relative measurements so that the declination angle and therefore the copters yaw angle have an absolute reference. This enables the length (but not the declination) of the earth field North/East states to change along with the magnetometer offsets.
2015-10-18 07:16:55 -03:00
/*
* Fuse declination angle using explicit algebraic equations generated with Matlab symbolic toolbox.
* The script file used to generate these and other equations in this filter can be found here:
* https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m
AP_NavEKF2: Update magnetometer fusion documentation
2015-10-18 19:37:47 -03:00
* This is used to prevent the declination of the EKF earth field states from drifting during operation without GPS
AP_NavEKF2: Allow use of magnetometer learning during optical flow nav Adds fusion of the declination when there are no earth relative measurements so that the declination angle and therefore the copters yaw angle have an absolute reference. This enables the length (but not the declination) of the earth field North/East states to change along with the magnetometer offsets.
2015-10-18 07:16:55 -03:00
* or some other absolute position or velocity reference
*/
AP_NavEKF2: reduce declination errors on start of 3-axis fusion Reset co-variances for NE field states.
2016-07-03 08:17:40 -03:00
void NavEKF2_core::FuseDeclination(float declErr)
AP_NavEKF2: Allow use of magnetometer learning during optical flow nav Adds fusion of the declination when there are no earth relative measurements so that the declination angle and therefore the copters yaw angle have an absolute reference. This enables the length (but not the declination) of the earth field North/East states to change along with the magnetometer offsets.
2015-10-18 07:16:55 -03:00
{
// declination error variance (rad^2)
AP_NavEKF2: reduce declination errors on start of 3-axis fusion Reset co-variances for NE field states.
2016-07-03 08:17:40 -03:00
const float R_DECL = sq(declErr);
AP_NavEKF2: Allow use of magnetometer learning during optical flow nav Adds fusion of the declination when there are no earth relative measurements so that the declination angle and therefore the copters yaw angle have an absolute reference. This enables the length (but not the declination) of the earth field North/East states to change along with the magnetometer offsets.
2015-10-18 07:16:55 -03:00
// copy required states to local variables
float magN = stateStruct.earth_magfield.x;
float magE = stateStruct.earth_magfield.y;
// prevent bad earth field states from causing numerical errors or exceptions
if (magN < 1e-3f) {
return;
}
// Calculate observation Jacobian and Kalman gains
AP_NavEKF2: Fix bug in magnetic heading and declination fusion equations. The derivation incorrectly used a tan instead of an atan function. This applies the corrected auto-code.
2016-02-16 01:41:48 -04:00
float t2 = magE*magE;
float t3 = magN*magN;
float t4 = t2+t3;
float t5 = 1.0f/t4;
float t22 = magE*t5;
float t23 = magN*t5;
float t6 = P[16][16]*t22;
float t13 = P[17][16]*t23;
float t7 = t6-t13;
float t8 = t22*t7;
float t9 = P[16][17]*t22;
float t14 = P[17][17]*t23;
float t10 = t9-t14;
float t15 = t23*t10;
float t11 = R_DECL+t8-t15; // innovation variance
AP_NavEKF2: prevent potential arithmetic error in declination fusion
2016-05-23 22:56:52 -03:00
if (t11 < R_DECL) {
return;
}
AP_NavEKF2: Fix bug in magnetic heading and declination fusion equations. The derivation incorrectly used a tan instead of an atan function. This applies the corrected auto-code.
2016-02-16 01:41:48 -04:00
float t12 = 1.0f/t11;
AP_NavEKF2: Allow use of magnetometer learning during optical flow nav Adds fusion of the declination when there are no earth relative measurements so that the declination angle and therefore the copters yaw angle have an absolute reference. This enables the length (but not the declination) of the earth field North/East states to change along with the magnetometer offsets.
2015-10-18 07:16:55 -03:00
float H_MAG[24];
AP_NavEKF2: Fix bug in magnetic heading and declination fusion equations. The derivation incorrectly used a tan instead of an atan function. This applies the corrected auto-code.
2016-02-16 01:41:48 -04:00
H_MAG[16] = -magE*t5;
H_MAG[17] = magN*t5;
AP_NavEKF2: Allow use of magnetometer learning during optical flow nav Adds fusion of the declination when there are no earth relative measurements so that the declination angle and therefore the copters yaw angle have an absolute reference. This enables the length (but not the declination) of the earth field North/East states to change along with the magnetometer offsets.
2015-10-18 07:16:55 -03:00
for (uint8_t i=0; i<=15; i++) {
AP_NavEKF2: Fix bug in magnetic heading and declination fusion equations. The derivation incorrectly used a tan instead of an atan function. This applies the corrected auto-code.
2016-02-16 01:41:48 -04:00
Kfusion[i] = -t12*(P[i][16]*t22-P[i][17]*t23);
AP_NavEKF2: Allow use of magnetometer learning during optical flow nav Adds fusion of the declination when there are no earth relative measurements so that the declination angle and therefore the copters yaw angle have an absolute reference. This enables the length (but not the declination) of the earth field North/East states to change along with the magnetometer offsets.
2015-10-18 07:16:55 -03:00
}
AP_NavEKF2: Fix bug in magnetic heading and declination fusion equations. The derivation incorrectly used a tan instead of an atan function. This applies the corrected auto-code.
2016-02-16 01:41:48 -04:00
Kfusion[16] = -t12*(t6-P[16][17]*t23);
Kfusion[17] = t12*(t14-P[17][16]*t22);
AP_NavEKF2: Allow use of magnetometer learning during optical flow nav Adds fusion of the declination when there are no earth relative measurements so that the declination angle and therefore the copters yaw angle have an absolute reference. This enables the length (but not the declination) of the earth field North/East states to change along with the magnetometer offsets.
2015-10-18 07:16:55 -03:00
for (uint8_t i=17; i<=23; i++) {
AP_NavEKF2: Fix bug in magnetic heading and declination fusion equations. The derivation incorrectly used a tan instead of an atan function. This applies the corrected auto-code.
2016-02-16 01:41:48 -04:00
Kfusion[i] = -t12*(P[i][16]*t22-P[i][17]*t23);
AP_NavEKF2: Allow use of magnetometer learning during optical flow nav Adds fusion of the declination when there are no earth relative measurements so that the declination angle and therefore the copters yaw angle have an absolute reference. This enables the length (but not the declination) of the earth field North/East states to change along with the magnetometer offsets.
2015-10-18 07:16:55 -03:00
}
// get the magnetic declination
float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0;
// Calculate the innovation
float innovation = atanf(t4) - magDecAng;
// limit the innovation to protect against data errors
if (innovation > 0.5f) {
innovation = 0.5f;
} else if (innovation < -0.5f) {
innovation = -0.5f;
}
// correct the covariance P = (I - K*H)*P
// take advantage of the empty columns in KH to reduce the
// number of operations
AP_NavEKF2: Apply optimisations to declination fusion covariance update These are the same type of optimisations that were successful with the magnetometer fusion
2015-10-20 00:23:32 -03:00
for (unsigned i = 0; i<=stateIndexLim; i++) {
for (unsigned j = 0; j<=15; j++) {
AP_NavEKF2: Allow use of magnetometer learning during optical flow nav Adds fusion of the declination when there are no earth relative measurements so that the declination angle and therefore the copters yaw angle have an absolute reference. This enables the length (but not the declination) of the earth field North/East states to change along with the magnetometer offsets.
2015-10-18 07:16:55 -03:00
KH[i][j] = 0.0f;
}
AP_NavEKF2: Apply optimisations to declination fusion covariance update These are the same type of optimisations that were successful with the magnetometer fusion
2015-10-20 00:23:32 -03:00
KH[i][16] = Kfusion[i] * H_MAG[16];
KH[i][17] = Kfusion[i] * H_MAG[17];
for (unsigned j = 18; j<=23; j++) {
AP_NavEKF2: Allow use of magnetometer learning during optical flow nav Adds fusion of the declination when there are no earth relative measurements so that the declination angle and therefore the copters yaw angle have an absolute reference. This enables the length (but not the declination) of the earth field North/East states to change along with the magnetometer offsets.
2015-10-18 07:16:55 -03:00
KH[i][j] = 0.0f;
}
}
AP_NavEKF2: Apply optimisations to declination fusion covariance update These are the same type of optimisations that were successful with the magnetometer fusion
2015-10-20 00:23:32 -03:00
for (unsigned j = 0; j<=stateIndexLim; j++) {
for (unsigned i = 0; i<=stateIndexLim; i++) {
KHP[i][j] = KH[i][16] * P[16][j] + KH[i][17] * P[17][j];
AP_NavEKF2: Allow use of magnetometer learning during optical flow nav Adds fusion of the declination when there are no earth relative measurements so that the declination angle and therefore the copters yaw angle have an absolute reference. This enables the length (but not the declination) of the earth field North/East states to change along with the magnetometer offsets.
2015-10-18 07:16:55 -03:00
}
}
AP_NavEKF2: add numerical error checking and isolation to mag decl fusion
2016-05-10 04:26:01 -03:00
// Check that we are not going to drive any variances negative and skip the update if so
bool healthyFusion = true;
for (uint8_t i= 0; i<=stateIndexLim; i++) {
if (KHP[i][i] > P[i][i]) {
healthyFusion = false;
AP_NavEKF2: Allow use of magnetometer learning during optical flow nav Adds fusion of the declination when there are no earth relative measurements so that the declination angle and therefore the copters yaw angle have an absolute reference. This enables the length (but not the declination) of the earth field North/East states to change along with the magnetometer offsets.
2015-10-18 07:16:55 -03:00
}
}
AP_NavEKF2: add numerical error checking and isolation to mag decl fusion
2016-05-10 04:26:01 -03:00
if (healthyFusion) {
// update the covariance matrix
for (uint8_t i= 0; i<=stateIndexLim; i++) {
for (uint8_t j= 0; j<=stateIndexLim; j++) {
P[i][j] = P[i][j] - KHP[i][j];
}
}
AP_NavEKF2: Allow use of magnetometer learning during optical flow nav Adds fusion of the declination when there are no earth relative measurements so that the declination angle and therefore the copters yaw angle have an absolute reference. This enables the length (but not the declination) of the earth field North/East states to change along with the magnetometer offsets.
2015-10-18 07:16:55 -03:00
AP_NavEKF2: add numerical error checking and isolation to mag decl fusion
2016-05-10 04:26:01 -03:00
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning.
ForceSymmetry();
ConstrainVariances();
// zero the attitude error state - by definition it is assumed to be zero before each observaton fusion
stateStruct.angErr.zero();
// correct the state vector
for (uint8_t j= 0; j<=stateIndexLim; j++) {
statesArray[j] = statesArray[j] - Kfusion[j] * innovation;
}
// the first 3 states represent the angular misalignment vector. This is
// is used to correct the estimated quaternion on the current time step
stateStruct.quat.rotate(stateStruct.angErr);
// record fusion health status
faultStatus.bad_decl = false;
} else {
// record fusion health status
faultStatus.bad_decl = true;
}
AP_NavEKF2: Allow use of magnetometer learning during optical flow nav Adds fusion of the declination when there are no earth relative measurements so that the declination angle and therefore the copters yaw angle have an absolute reference. This enables the length (but not the declination) of the earth field North/East states to change along with the magnetometer offsets.
2015-10-18 07:16:55 -03:00
}
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
/********************************************************
* MISC FUNCTIONS *
********************************************************/
// align the NE earth magnetic field states with the published declination
void NavEKF2_core::alignMagStateDeclination()
{
AP_NavEKF2: remember mag field states between flight on same power cycle Remember the mag bias and earth field states learned during flight when the vehicle lands. This improves performance for vehicles that do multiple flight on one power cycle
2016-07-02 05:46:42 -03:00
// don't do this if we already have a learned magnetic field
if (magFieldLearned) {
return;
}
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
// get the magnetic declination
float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0;
// rotate the NE values so that the declination matches the published value
Vector3f initMagNED = stateStruct.earth_magfield;
AP_Math: Replace the pythagorous* functions with a variadic template The new function can deal with a variable number of function parameters. Additionally, I renamed the functions to norm(), because this is the standard name used in several other projects.
2016-04-16 06:58:46 -03:00
float magLengthNE = norm(initMagNED.x,initMagNED.y);
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
stateStruct.earth_magfield.x = magLengthNE * cosf(magDecAng);
stateStruct.earth_magfield.y = magLengthNE * sinf(magDecAng);
AP_NavEKF2: reduce declination errors on start of 3-axis fusion Reset co-variances for NE field states.
2016-07-03 08:17:40 -03:00
AP_NavEKF2: fix bug in initialisation of declination co-variances Co-variances were being re-zeroed after being set. This meant that the initial declination learning was sensitive to measurement errors which could result in poor initial yaw accuracy.
2016-07-04 10:54:28 -03:00
if (!inhibitMagStates) {
// zero the corresponding state covariances if magnetic field state learning is active
float var_16 = P[16][16];
float var_17 = P[17][17];
zeroRows(P,16,17);
zeroCols(P,16,17);
P[16][16] = var_16;
P[17][17] = var_17;
// fuse the declination angle to establish covariances and prevent large swings in declination
// during initial fusion
FuseDeclination(0.1f);
}
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
}
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
// record a magentic field state reset event
void NavEKF2_core::recordMagReset()
{
magStateInitComplete = true;
if (inFlight) {
AP_NavEKF2: Improve ground based magnetic anomaly protection for copter The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe. A check for yaw errors due to a ground based magnetic anomaly has been introduced. The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
2016-06-09 20:19:28 -03:00
finalInflightMagInit = true;
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
}
AP_NavEKF2: Improve ground based magnetic anomaly protection for copter The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe. A check for yaw errors due to a ground based magnetic anomaly has been introduced. The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
2016-06-09 20:19:28 -03:00
// take a snap-shot of the vertical position, quaternion and yaw innovation to use as a reference
// for post alignment checks
posDownAtLastMagReset = stateStruct.position.z;
quatAtLastMagReset = stateStruct.quat;
yawInnovAtLastMagReset = innovYaw;
AP_NavEKF2: rework yaw and magnetic heading reset logic Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 03:02:52 -03:00
}
AP_NavEKF2: split up EKF_core into different files
2015-10-05 23:30:07 -03:00
AP_NavEKF2: Update function header comments
2015-10-17 20:32:11 -03:00
#endif // HAL_CPU_CLASS