EKF: Add Emergency yaw recovery using EKF-GSF estimator #766

priseborough · 2020-03-04T02:54:46Z

Replaces #764.

Has been rebased.

See #764. for discussion and test evidence

This reverts commit 9300530.

Also remove unnecessary operations

Use conversion from rotation matrix to Euler angles instead of quaternion to euler angles.

memcpy bypasses compiler sanity checks and is unnecessary in this instance.

Adds a common function to support yaw reset that can be used elsewhere.

kamilritz

Thanks a lot for all your work.

kamilritz · 2020-03-04T06:24:28Z

EKF/EKFGSF_yaw.cpp

+	_gsf_yaw = 0.0f;
+}
+
+void EKFGSF_yaw::update(const Vector3f del_ang, // IMU delta angle rotation vector meassured in body frame (rad)


Would pass these vectors as a const-reference. Or even better pass an imuSample as const-reference.

Then you have less input arguments, which makes it more readable.

kamilritz · 2020-03-04T06:32:55Z

EKF/EKFGSF_yaw.cpp

+
+			// normalise the weighting function
+			if (_ekf_gsf_vel_fuse_started && total_w > 1e-6f) {
+				float total_w_inv = 1.0f / total_w;


Suggested change

float total_w_inv = 1.0f / total_w;

const float total_w_inv = 1.0f / total_w;

kamilritz · 2020-03-04T06:33:18Z

EKF/EKFGSF_yaw.cpp

+				}
+			} else {
+				// assign even weights
+				float default_weight = 1.0f / (float)N_MODELS_EKFGSF;


Suggested change

float default_weight = 1.0f / (float)N_MODELS_EKFGSF;

const float default_weight = 1.0f / (float)N_MODELS_EKFGSF;

kamilritz · 2020-03-04T06:34:51Z

EKF/EKFGSF_yaw.cpp

+	// Calculate a composite yaw vector as a weighted average of the states for each model.
+	// To avoid issues with angle wrapping, the yaw state is converted to a vector with legnth
+	// equal to the weighting value before it is summed.
+	Vector2f yaw_vector = {};


Suggested change

Vector2f yaw_vector = {};

Vector2f yaw_vector;

Is filled with zero automatically

EKF/EKFGSF_yaw.h

kamilritz · 2020-03-04T06:58:39Z

EKF/EKFGSF_yaw.cpp

+	return false;
+}
+
+void EKFGSF_yaw::setVelocity(Vector2f velocity, float accuracy)


Suggested change

void EKFGSF_yaw::setVelocity(Vector2f velocity, float accuracy)

void EKFGSF_yaw::setVelocity(const Vector2f &velocity, float accuracy)

kamilritz · 2020-03-04T06:59:44Z

EKF/EKFGSF_yaw.h

+#define N_MODELS_EKFGSF 5
+
+#ifndef M_PI_F
+#define M_PI_F 3.14159265f
+#endif
+
+#ifndef M_PI_2_F
+#define M_PI_2_F 1.57079632f
+#endif
+
+#ifndef M_TWOPI_INV
+#define M_TWOPI_INV 0.159154943f
+#endif


If some of them are not define else where, I would define them as const float/int

kamilritz · 2020-03-04T07:01:34Z

EKF/EKFGSF_yaw.h

+		matrix::Vector3f X; // Vel North (m/s),  Vel East (m/s), yaw (rad)s
+		matrix::SquareMatrix<float, 3> P; // covariance matrix
+		float W = 0.0f; // weighting
+		matrix::SquareMatrix<float, 2> S; // innovation covariance matrix


I think we could get away without storing S, but only its inverse. Just an idea.

kamilritz · 2020-03-04T07:06:24Z

EKF/ekf_helper.cpp

+
+}
+
+void Ekf::requestEmergencyNavReset(uint8_t counter)


If the client can decide, if he wants to increase the counter or not. He is also able to decide if he want to call this function or not. Therefore the counter argument is not necessary. You can have an internal counter.

kamilritz · 2020-03-04T07:07:26Z

EKF/ekf_helper.cpp

+		Vector2f vel_NE = Vector2f(_gps_sample_delayed.vel(0),_gps_sample_delayed.vel(1));
+		yawEstimator.setVelocity(vel_NE, _gps_sample_delayed.vacc);


Suggested change

Vector2f vel_NE = Vector2f(_gps_sample_delayed.vel(0),_gps_sample_delayed.vel(1));

yawEstimator.setVelocity(vel_NE, _gps_sample_delayed.vacc);

yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.vacc);

jkflying

I've taken read over and I don't see anything which looks like a big problem. There's a lot of value here, and it would be great to get this into the firmware immediately after the 1.11 branch-off (next week) to give it maximum testing for 1.12.

@kamilritz I think a lot of these issues you could fix quicker yourself, and don't affect safety/performance/functionality, they are more aesthetic or tiny performance gain. Why don't we get this PR in and you can make a follow-up PR with the remaining cleanup things. Is that OK with you?

priseborough · 2020-03-05T01:21:53Z

Here are before and after replay output for the last two commits 37572b3 and 5270fde
showing the individual innovation magnitudes and the yaw estimates. The innovation levels during manoeuvres are lower overall and the EKF solution index 3 that initialises near the reciprocal heading converges faster:

Replay of multirotor flight with manoeuvring in stabilise mode:

Before:

After:

This is the effect of the changes applied to the stable release version and used to replay a VTOL log with higher levels of vibration and a dynamic transition manoeuvre.

Before:

After:

kamilritz · 2020-03-05T06:54:24Z

@jkflying Your idea is fine with me. I can open a follow-up PR with some style changes. But I would rely on @priseborough for the functional testing part. Is that okay?
I also have not spotted any critical issue. Lets get this squashed and merged!

LorenzMeier · 2020-03-09T11:23:33Z

@ndepal had a flight that looks like he needs this feature: https://logs.px4.io/plot_app?log=c231b7b3-ab35-4f48-be8c-0cb0a836cae4

priseborough added 30 commits March 4, 2020 13:35

EKF: Use common rate vector calculation for offset corrections

195f167

EKF: Remove duplicate matrix entry calculations

af84e30

EKF: Create a EKF-GSF yaw estimator class

14e1924

EKF: add emergency yaw reset functionality

c8c1904

EKF: remove un-used function

a566eb0

EKF: Ensure required constants are defined for all builds

c59daad

EKF: Fix CI build error

e9dd2ef

Revert "EKF: remove un-used function"

6734251

This reverts commit 9300530.

EKF: Replace in-lined Tait-Bryan 312 conversions with function call

1608afa

Also remove unnecessary operations

EKF: Remove unnecessary update of external vision rotation matrix

ca2a0e5

EKF: Use const

dbb5230

EKF: use const

a9a2004

EKF: don't use class variable as a temporary variable

b254488

EKF: update comments

bdc2b47

EKF: Improve efficiency of yaw reset

7279978

Use conversion from rotation matrix to Euler angles instead of quaternion to euler angles.

EKF: use const

7f62269

EKF: remove un-used struct element

22e316a

EKF: more descriptive function name

be72afe

EKF: use existing matrix row operator

3ae091e

EKF: remove unnecessary rotation matrix update

26fad77

EKF: Use square matrix type

c449153

EKF: Improve protection for bad innovation covariance

e7b9843

EKF: Use matrix library operations

94132c5

EKF: Replace memcpy with better alternative

68303f9

memcpy bypasses compiler sanity checks and is unnecessary in this instance.

EKF: Split EKF-GSF yaw reset function

9a46329

Adds a common function to support yaw reset that can be used elsewhere.

EKF: Use common function for quaternion state and covariance yaw reset

78ea53b

EKF: Replace inlined matrix operation

d1a85a7

EKF: Use const

3d58ca5

EKF: Change accessor function name

14035f7

EKF: Use const

7032c73

priseborough added 4 commits March 4, 2020 13:35

EKF: Don't create unnecessary duplicate variable locations

1c5ac7c

EKF: Remove duplicate covariance innovation inverse

c6216eb

EKF: Don't create unnecessary duplicate variable locations

9b69745

EKF: Rely on geo library to provide gravity

d6aa205

priseborough requested review from bresch and kamilritz March 4, 2020 02:54

priseborough mentioned this pull request Mar 4, 2020

EKF: Add Emergency yaw recovery using EKF-GSF estimator #764

Closed

kamilritz suggested changes Mar 4, 2020

View reviewed changes

EKF: Improve protection from bad updates

b31a21e

priseborough force-pushed the yawEKFGSF-refactor-rebased branch from 0ade564 to b31a21e Compare March 4, 2020 12:14

jkflying previously approved these changes Mar 4, 2020

View reviewed changes

priseborough added 2 commits March 5, 2020 11:44

EKF: Reduce effect of vibration on yaw estimator AHRS

37572b3

EKF: Improve yaw estimator AHRS accuracy during manoeuvre transients

5270fde

priseborough dismissed jkflying’s stale review via 5270fde March 5, 2020 01:12

jkflying approved these changes Mar 5, 2020

View reviewed changes

kamilritz approved these changes Mar 5, 2020

View reviewed changes

priseborough merged commit 4669aa6 into PX4:master Mar 5, 2020

CarlOlsson mentioned this pull request Mar 9, 2020

Vehicle crash due do estimator GPS failure #771

Closed

mrpollo mentioned this pull request Mar 16, 2020

v1.11.0 Release Notes PX4/PX4-Autopilot#14402

Closed

priseborough added ekf enhancement labels Jun 27, 2020

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

EKF: Add Emergency yaw recovery using EKF-GSF estimator #766

EKF: Add Emergency yaw recovery using EKF-GSF estimator #766

priseborough commented Mar 4, 2020

kamilritz left a comment

kamilritz Mar 4, 2020

kamilritz Mar 4, 2020

kamilritz Mar 4, 2020

kamilritz Mar 4, 2020

kamilritz Mar 4, 2020

kamilritz Mar 4, 2020

kamilritz Mar 4, 2020

kamilritz Mar 4, 2020

kamilritz Mar 4, 2020

kamilritz Mar 4, 2020

kamilritz Mar 4, 2020

jkflying left a comment

priseborough commented Mar 5, 2020 •

edited

Loading

kamilritz commented Mar 5, 2020

LorenzMeier commented Mar 9, 2020

	float total_w_inv = 1.0f / total_w;
	const float total_w_inv = 1.0f / total_w;

	float default_weight = 1.0f / (float)N_MODELS_EKFGSF;
	const float default_weight = 1.0f / (float)N_MODELS_EKFGSF;

	void EKFGSF_yaw::setVelocity(Vector2f velocity, float accuracy)
	void EKFGSF_yaw::setVelocity(const Vector2f &velocity, float accuracy)

		Vector2f vel_NE = Vector2f(_gps_sample_delayed.vel(0),_gps_sample_delayed.vel(1));
		yawEstimator.setVelocity(vel_NE, _gps_sample_delayed.vacc);

	Vector2f vel_NE = Vector2f(_gps_sample_delayed.vel(0),_gps_sample_delayed.vel(1));
	yawEstimator.setVelocity(vel_NE, _gps_sample_delayed.vacc);
	yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.vacc);

EKF: Add Emergency yaw recovery using EKF-GSF estimator #766

EKF: Add Emergency yaw recovery using EKF-GSF estimator #766

Conversation

priseborough commented Mar 4, 2020

kamilritz left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jkflying left a comment

Choose a reason for hiding this comment

priseborough commented Mar 5, 2020 • edited Loading

kamilritz commented Mar 5, 2020

LorenzMeier commented Mar 9, 2020

priseborough commented Mar 5, 2020 •

edited

Loading