AP_ExternalAHRS: added ANU CINS state estimator #24747

tridge · 2023-08-24T04:51:47Z

This is the ANU CINS estimator worked on by @patwilt and @pvangoor
This is still experimental but is giving promising results in test flights

remaining issues:

using std::queue, should use ArduPilot queue APIs
should support baro for height for handling GPS outage
add option to not use compass

peterbarker

stack-frame-too-big on CubeOrange

libraries/AP_CINS/AP_CINS.h

peterbarker · 2024-09-11T09:29:38Z

libraries/AP_Math/LieGroups.cpp

@@ -0,0 +1,173 @@
+#include "AP_Math.h"
+#include "LieGroups.h"


Links to papers required!

Can we upload them to the repo so if hosting goes down, we still have access?

libraries/AP_Math/LieGroups.h

libraries/AP_Math/matrix3.cpp

tridge · 2024-10-30T22:18:53Z

@peterbarker stack size issues fixed

when using --enable-FEATURE also enable dependent feattures for 1 level

resvolved

peterbarker · 2024-11-01T04:24:39Z

libraries/AP_CINS/AP_CINS.cpp

+    state.XHat = Gal3F::identity();
+    state.ZHat = SIM23::identity();


Usually we'd do state.XHat.identity()

peterbarker · 2024-11-01T04:27:06Z

libraries/AP_CINS/AP_CINS.cpp

+    state.acc_bias_gain_mat.rot.zero();
+    state.acc_bias_gain_mat.vel.zero();
+    state.acc_bias_gain_mat.pos.zero();


These are all implicitly zero; if this was a reset we'd need these.

peterbarker · 2024-11-01T04:32:27Z

libraries/AP_CINS/AP_CINS.cpp

+    // see if we have new GPS data
+    const auto &gps = dal.gps();
+    if (gps.status() >= AP_DAL_GPS::GPS_OK_FIX_3D) {
+        const uint32_t last_gps_fix_ms = gps.last_message_time_ms(0);


Mismatch between this and the gps.location() line

peterbarker

I wonder if we should run this through the code formatter before putting it in.

peterbarker · 2024-11-01T07:55:29Z

Tools/ardupilotwaf/boards.py

@@ -167,6 +167,12 @@ def srcpath(path):
            disable_option = "disable_" + enable_option[len("enable-"):]
            if getattr(cfg.options, enable_option, False):
                env.CXXFLAGS += ['-D%s=1' % opt.define]
+                if opt.dependency:
+                    for d in opt.dependency.split(','):


Lift method from build_options.py?

peterbarker · 2024-11-01T07:56:55Z

Tools/scripts/build_options.py

-    Feature('AHRS', 'EKF3_WINDEST', 'EK3_FEATURE_DRAG_FUSION', 'Enable Wind estimation for EKF3', 0, 'EKF3'),
-    Feature('AHRS', 'EKF3_OPTFLOW', 'EK3_FEATURE_OPTFLOW_FUSION', 'Enable OpticalFlow fusion for EKF3', 0, 'EKF3,OPTICALFLOW'),
-    Feature('AHRS', 'BARO_WIND_COMP', 'HAL_BARO_WIND_COMP_ENABLED', 'Enable Baro wind compensation', 0, None),
+    Feature('AHRS', 'EKF3_EXTNAV', 'EK3_FEATURE_EXTERNAL_NAV', 'Enable External Navigation for EKF3', 0, 'EKF3'),


case problems

peterbarker · 2024-11-01T07:57:33Z

libraries/AP_CINS/AP_CINS.cpp

@@ -0,0 +1,801 @@
+/*
+  CINS state estimator for AP_AHRS, developed by Mr Patrick Wiltshire and Dr Pieter Van Goor
+ */


copyright/license required?

peterbarker · 2024-11-01T07:57:59Z

libraries/AP_CINS/AP_CINS.cpp

+  CINS state estimator for AP_AHRS, developed by Mr Patrick Wiltshire and Dr Pieter Van Goor
+ */
+
+#include "AP_CINS.h"


Suggested change

#include "AP_CINS.h"

#include "AP_CINS_config.h"

peterbarker · 2024-11-01T07:58:19Z

libraries/AP_CINS/AP_CINS.cpp

+#include <GCS_MAVLink/GCS.h>
+
+// gains tested for 5Hz GPS
+#define CINS_GAIN_GPSPOS_ATT (1.0E-5)


Just inline these into the parameter definitions?

peterbarker · 2024-11-01T08:16:15Z

libraries/AP_CINS/AP_CINS.cpp

+    // The key point is that Omega changes R which changes Omega.
+    // We use a first order expansion to capture this effect here.
+
+    const Vector3F omega1 = - Matrix3F::skew_symmetric(v1) * v2 * gain;


Suggested change

const Vector3F omega1 = - Matrix3F::skew_symmetric(v1) * v2 * gain;

const auto skewed_v1 = Matrix3F::skew_symmetric(v1);

const Vector3F omega1 = - skewed_v1 * v2 * gain;

and use this elsewhere.

similarly for skewed omega1

peterbarker · 2024-11-01T08:16:37Z

libraries/AP_CINS/AP_CINS.cpp

+    const Vector3F omega2 = - Matrix3F::skew_symmetric(v1) * Matrix3F::skew_symmetric(omega1) * v2 * gain;
+    const Vector3F omega3 = - Matrix3F::skew_symmetric(v1) * Matrix3F::skew_symmetric(omega2) * v2 * gain
+                            - Matrix3F::skew_symmetric(v1) * Matrix3F::skew_symmetric(omega1) * Matrix3F::skew_symmetric(omega1) * v2 * gain;
+    return omega1 + omega2*dt + omega3*0.5*dt*dt;


Suggested change

return omega1 + omega2*dt + omega3*0.5*dt*dt;

return omega1 + omega2*dt + omega3*0.5*sq(dt);

peterbarker · 2024-11-01T08:17:46Z

libraries/AP_CINS/AP_CINS.h

+            ftype magTest;
+        } variances;
+
+    } state;


Not sure why we have a state structure

peterbarker · 2024-11-01T08:20:29Z

libraries/AP_ExternalAHRS/AP_ExternalAHRS_CINS.h

+
+    // accessors for AP_AHRS
+    bool healthy(void) const override {
+        return true;


Can we do better than this?!

peterbarker · 2024-11-01T08:20:52Z

libraries/AP_ExternalAHRS/AP_ExternalAHRS_CINS.h

+        return true;
+    }
+    bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override {
+        return true;


Even if we have never had a GPS measurement?

tridge added the WIP label Aug 24, 2023

tridge force-pushed the pr-AHRS_CINS branch from 240992a to 411b987 Compare September 15, 2023 21:19

tridge force-pushed the pr-AHRS_CINS branch 2 times, most recently from 451699b to 6dbce7b Compare November 24, 2023 03:36

tridge force-pushed the pr-AHRS_CINS branch 3 times, most recently from c9e1e57 to 0b2dc4e Compare May 10, 2024 08:25

tridge force-pushed the pr-AHRS_CINS branch from 42b4d16 to c802198 Compare August 11, 2024 05:00

tridge force-pushed the pr-AHRS_CINS branch from 553a0b3 to 183357d Compare September 11, 2024 04:29

tridge added the DevCallEU label Sep 11, 2024

tridge changed the title ~~AP_ExternalAHRS: added ANU CINS state estimator (WIP)~~ AP_ExternalAHRS: added ANU CINS state estimator Sep 11, 2024

tridge removed the WIP label Sep 11, 2024

peterbarker previously requested changes Sep 11, 2024

View reviewed changes

tridge force-pushed the pr-AHRS_CINS branch from 183357d to bb3bc4c Compare September 11, 2024 09:43

peterbarker removed the DevCallEU label Sep 11, 2024

tridge force-pushed the pr-AHRS_CINS branch 2 times, most recently from 6f9ff1b to 168d6ca Compare October 14, 2024 08:00

tridge mentioned this pull request Oct 15, 2024

ExternalAHRS: support variances in backends #28405

Merged

tridge force-pushed the pr-AHRS_CINS branch 3 times, most recently from c8d0b6a to 31f9d8f Compare October 15, 2024 03:57

tridge added DevCallEU ExternalAHRS labels Oct 15, 2024

tridge force-pushed the pr-AHRS_CINS branch 3 times, most recently from 58d5de6 to 516069c Compare October 19, 2024 23:45

tridge force-pushed the pr-AHRS_CINS branch from 516069c to 0135740 Compare October 26, 2024 23:32

tridge added 2 commits October 31, 2024 07:17

waf: added AP_CINS library

456b63f

Copter: cope with AHRS backends with no variances

26f5560

tridge force-pushed the pr-AHRS_CINS branch from 0135740 to d6915ff Compare October 30, 2024 22:18

tridge force-pushed the pr-AHRS_CINS branch 2 times, most recently from aba5014 to 38a09b8 Compare October 31, 2024 22:30

patwilt and others added 8 commits November 1, 2024 13:02

AP_Math: Added LieGroups implementation

950ee0e

AP_CINS: added ANU CINS state estimator

2c50156

AP_ExternalAHRS: added CINS backend

837ca65

Tools: support ExternalAHRS for Replay

7c64f82

Tools: added feature selection for CINS

d45dc8d

Tools: make CINS depend on AHRS_EXT

610cc28

waf: added 1 level recursive depedencies

efc5335

when using --enable-FEATURE also enable dependent feattures for 1 level

Tools: don't build replay on low-end STM32 boards

6ae6607

tridge force-pushed the pr-AHRS_CINS branch from 38a09b8 to 6ae6607 Compare November 1, 2024 04:03

peterbarker reviewed Nov 1, 2024

View reviewed changes

peterbarker requested changes Nov 1, 2024

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

AP_ExternalAHRS: added ANU CINS state estimator #24747

AP_ExternalAHRS: added ANU CINS state estimator #24747

tridge commented Aug 24, 2023 •

edited

Loading

peterbarker left a comment

peterbarker Sep 11, 2024

Ryanf55 Sep 11, 2024

tridge commented Oct 30, 2024

peterbarker Nov 1, 2024

peterbarker Nov 1, 2024

peterbarker Nov 1, 2024

peterbarker left a comment

peterbarker Nov 1, 2024

peterbarker Nov 1, 2024

peterbarker Nov 1, 2024

peterbarker Nov 1, 2024

peterbarker Nov 1, 2024

peterbarker Nov 1, 2024

peterbarker Nov 1, 2024

peterbarker Nov 1, 2024

peterbarker Nov 1, 2024

peterbarker Nov 1, 2024

		state.XHat = Gal3F::identity();
		state.ZHat = SIM23::identity();

	const Vector3F omega1 = - Matrix3F::skew_symmetric(v1) * v2 * gain;
	const auto skewed_v1 = Matrix3F::skew_symmetric(v1);
	const Vector3F omega1 = - skewed_v1 * v2 * gain;

	return omega1 + omega2dt + omega30.5dtdt;
	return omega1 + omega2dt + omega30.5*sq(dt);

AP_ExternalAHRS: added ANU CINS state estimator #24747

Are you sure you want to change the base?

AP_ExternalAHRS: added ANU CINS state estimator #24747

Conversation

tridge commented Aug 24, 2023 • edited Loading

peterbarker left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

tridge commented Oct 30, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

peterbarker 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

tridge commented Aug 24, 2023 •

edited

Loading