Pullrequest relative localization filter #2262

coppolam · 2018-05-08T11:13:02Z

This is a pull request for a relative localization filter that uses Ultra Wide Band (or any range sensor) to compute the relative location of other MAVs. The code has been tested to work with decawave_anchorless_communication.c (with the changes included).

This pull request is accompanied by a pull request in pprzlink to add the message RLFILTER

kirkscheper · 2018-05-09T14:38:16Z

conf/modules/relative_localization_filter.xml

+<module name="relative_localization_filter" dir="relative_localization_filter">
+	<doc>
+		<description>
+			Relative Localization Filter based on UWB range measurements + Communication between drones.


can you update spacing here to be similar to other projects?

kirkscheper · 2018-05-09T14:40:15Z

sw/airborne/modules/relative_localization_filter/discrete_ekf.c

+
+  // Q Matrix
+  MAKE_MATRIX_PTR(_Q, filter->Q, EKF_N);
+  float_mat_identity_scal(_Q, pow(0.3, 2.0), EKF_N);


For float constants like this, we prefer 2.f to 2.0 (the latter is a double by default).

kirkscheper · 2018-05-09T15:31:23Z

sw/airborne/modules/relative_localization_filter/discrete_ekf.h

+#define EKF_N 7
+#define EKF_M 6
+
+typedef struct discrete_ekf {


We don't use type defs in paparazzi. Please remove and use 'struct discrete_ekf'.

kirkscheper · 2018-05-09T15:31:48Z

sw/airborne/modules/relative_localization_filter/relative_localization_filter.h

+
+#include "discrete_ekf.h"
+
+extern discrete_ekf ekf_rl[RL_NUAVS - 1];


why is this defined here? Is this parameter used outside of the relative_localization_filter source file? Only include things needed by other files in the header.

The idea is that, if desired, this can be directly read out from other modules so that they can do something with the info. I could also change to an ABI message though, if that's preferred in paparazzi.

kirkscheper · 2018-05-09T16:15:45Z

Yup!

kirkscheper · 2018-05-13T09:34:57Z

sw/airborne/math/pprz_algebra_float.h

@@ -673,6 +673,18 @@ static inline void float_mat_transpose(float **a, int n)
  }
 }

+
+/** transpose non-square matrix */
+static inline void float_mat_transpose_general(float **o, float **a, int n, int m)


Seems like it would be better to have float_mat_transpose and float_mat_transpose_square. That would require you to update all uses of this function but it seems better.

kirkscheper · 2018-05-13T09:42:09Z

sw/airborne/math/pprz_algebra_float.h

+ * b: [n x l]
+ * o: [m x l]
+ */
+static inline void float_mat_vect_mul(float *o, float **a, float *b, int m, int n)


Can you update the comments to be 1 instead of l? You also need to set o[i] to zero before the second for loop.

kirkscheper · 2018-05-13T09:55:18Z

sw/airborne/math/pprz_algebra_float.h

@@ -734,15 +763,14 @@ static inline void float_mat_col(float *o, float **a, int m, int c)
 }

 /** Make an n x n identity matrix (for matrix passed as array) */
-static inline void float_mat_identity(float **o, int n)
+static inline void float_mat_identity_scal(float **o, float v, int n)


This is not an identity matrix if the values are non-zero...

You can either use float_mat_identity and float_mat_vect_mul or make a new function e.g. float_mat_diagonal_scal. But given that you only use non-one values in a function initialization it's probably not strictly necessary, up to you.

kirkscheper · 2018-05-13T10:00:45Z

sw/airborne/modules/decawave/decawave_anchorless_communication.c

@@ -32,7 +32,7 @@
  on-board values with each-other (for purposes such as relative localization, co-ordination, or collision avoidance).
  This module must be used together with the Decawave DWM1000 running the appropriate Serial Communication code, which can be flashed on the Arduino board.
  The arduino library can be found at:
-    https://github.com/StevenH2812/arduino-dw1000/tree/UWB_localization_v1.0
+    https://github.com/StevenH2812/arduino-dw1000/tree/UWB_onboard


its probably a good idea to get a copy of this on the tudelft repo or have Steven transfer ownership and use that repo.

Yes, that is on the to-do list, at which point we can change the link

As discussed a pull request will be created for all the fixes and addition to the original repo here https://github.com/thotro

Note that this is separate from this pull request, this one can, after fixes suggested by @kirkscheper as reviewer are implemented) Which as of 20180530 they are as understood from @coppolam

note that this s only a remark line where one could find the arduino-dw1000 UWB_onboard code, and so far a valid one... so no blocker for a pull since there is no other URL atm, as soon as the pull and merge with trhe originating repo is availabe, feel free to adjust the remark to another url...

kirkscheper · 2018-05-13T10:04:32Z

sw/airborne/modules/relative_localization_filter/relative_localization_filter.c

+uint8_t number_filters;
+struct discrete_ekf ekf_rl[RL_NUAVS - 1];
+float range_array[RL_NUAVS - 1];
+uint8_t pprzmsg_cnt;


Can you add a short description for these global variables?

kirkscheper · 2018-05-13T10:08:07Z

sw/airborne/modules/relative_localization_filter/relative_localization_filter.c

+
+void relative_localization_filter_init(void)
+{
+  int32_vect_set_value(ID_array, 5, RL_NUAVS - 1);


What is 5 here? Not clear what happening or why. I guess this should be RL_NUAVS.

kirkscheper · 2018-05-13T10:08:53Z

sw/airborne/modules/relative_localization_filter/relative_localization_filter.h

+#include "discrete_ekf.h"
+
+// This can be used elsewhere to directly read out the result of the ekf
+extern struct discrete_ekf ekf_rl[RL_NUAVS - 1];


Please replace this with abi messages if you want to share this data with other modules.

Like this, any code which reads out the data will be dependent on this module and not work with any other module. If you utilize the abi messaging structure, any modules which use this relative localization estimation could use other estimation methods as well (e.g adsb). This makes the module structure more flexible and useful.

kirkscheper · 2018-05-13T10:13:41Z

sw/airborne/modules/relative_localization_filter/relative_localization_filter.c

+#include <stdio.h>
+#include <stdlib.h>
+
+int32_t ID_array[RL_NUAVS - 1];


Why id in caps?

kirkscheper · 2018-05-13T10:15:41Z

sw/airborne/modules/relative_localization_filter/relative_localization_filter.h

+#define RELATIVE_LOCALIZATION_FILTER_H
+
+#ifndef RL_NUAVS
+#define RL_NUAVS 5 // Maximum expected number of UAVs


why not define this as the maximum number of expected 'other' UAVs, then you don't have to constantly have RL_NUAVS - 1.

kirkscheper · 2018-05-13T10:19:22Z

sw/airborne/modules/decawave/decawave_anchorless_communication.c

@@ -262,6 +285,10 @@ void decawave_anchorless_communication_periodic(void)
  sendFloat(UWB_SERIAL_COMM_VX, stateGetSpeedEnu_f()->y);
  sendFloat(UWB_SERIAL_COMM_VY, stateGetSpeedEnu_f()->x);
  sendFloat(UWB_SERIAL_COMM_Z, stateGetPositionEnu_f()->z);
+  sendFloat(UWB_SERIAL_COMM_AX, 0.);
+  sendFloat(UWB_SERIAL_COMM_AY, 0.);
+  sendFloat(UWB_SERIAL_COMM_YAWR, 0.);


Interesting. Is this some functionality that will be added later?

kirkscheper

Almost there!

kirkscheper · 2018-05-14T11:53:17Z

sw/airborne/modules/relative_localization_filter/relative_localization_filter.c

+  pprzmsg_cnt++;
+  if (pprzmsg_cnt >= number_filters) {
+    pprzmsg_cnt = 0;
+  }


sorry to nag here but what if number_filters=0? Won't this message will contain gibberish.

kirkscheper · 2018-05-14T11:53:43Z

sw/airborne/modules/relative_localization_filter/relative_localization_filter.c

+    // send id, x, y, z, vx, vy
+    AbiSendMsgRELATIVE_LOCALIZATION_EKF(RL_EKF_ID, id_array[i], ekf_rl[i].X[0], ekf_rl[i].X[1], ekf_rl[i].X[6], ekf_rl[i].X[4], ekf_rl[i].X[5]);
+  }
+};


add new line here.

OpenUAS

All code changes requested are no implemented, nice..

gautierhattenberger · 2018-05-30T20:52:16Z

conf/abi.xml

@@ -156,6 +156,15 @@
      <field name="accel_sp" type="struct FloatVect3 *" unit="m/s^2"/>
    </message>

+    <message name="RELATIVE_LOCALIZATION_EKF" id="24">


Who is using this message ? There is no bind for it apparently.

Also, it would be nicer to have this more generic. Several options:

remove "_EKF"

add "vz" even if not used in this case

replace this by a combination of POSITION_ESTIMATE and VELOCITY_ESTIMATE (which means that an extra ID field might be required)

Indeed, this message is not used anywhere at the moment. It was created so that it can be used by other modules that wish to do something with the relative localization data.

…ed that it builds

…t_transpose_general --> float_mat_transpose

…r observed

…readings to uwb message

…dded explanation

…rz message + changed float_mat_identity_scal to float_mat_diag_scal

…sage + added a few comments + tested

gautierhattenberger · 2018-06-06T17:10:33Z

I have merged the corresponding message in pprzlink, please update the reference accordingly.

* added files (tested) * fixed code style * removed RLFILTER from default_rotorcraft.xml * fixed xml file, changed 2.0 to .f for floats, removed typedef + checked that it builds * fixed float_mat_transpose --> float_mat_transpose_square and float_mat_transpose_general --> float_mat_transpose * pprz_algebra_float set o[i]=0 at line 717 * [relative_localization_filter] RL_NUAVS changed from all to just other observed * [relative_localization_filter] ID_array -> id_array * [decawave_anchorless_communication] added acceleration and yaw rates readings to uwb message * [relative_localization_filter] line 91, changed 5 to RL_NUAVS+1 and added explanation * [relative_localization_filter] added comments to global variables * [relative_localization_filter] moved ekf_rl to c file * [relative_localization_filter] added abi message * [relative_localization_filter] fixed abi message + check build * [relative_localization_filter] added line end + added if guard too pprz message + changed float_mat_identity_scal to float_mat_diag_scal * [relative_localization_filter] updated ABI name + added vz to ABI message + added a few comments + tested * update pprzlink reference to include RLFILTER message

kirkscheper reviewed May 9, 2018

View reviewed changes

coppolam force-pushed the pullrequest_relative_localization_filter branch from df5041a to fc3424d Compare May 9, 2018 15:59

kirkscheper requested changes May 13, 2018

View reviewed changes

kirkscheper reviewed May 14, 2018

View reviewed changes

OpenUAS self-requested a review May 30, 2018 16:35

OpenUAS assigned coppolam May 30, 2018

OpenUAS added communication Airborne Enhancement Improve upon exiting functionality Module TestingRequired and removed communication labels May 30, 2018

OpenUAS added this to the v5.14 milestone May 30, 2018

OpenUAS approved these changes May 30, 2018

View reviewed changes

gautierhattenberger reviewed May 30, 2018

View reviewed changes

coppolam force-pushed the pullrequest_relative_localization_filter branch from 151a406 to 9df6c55 Compare May 31, 2018 08:54

coppolam added 13 commits May 31, 2018 11:04

added files (tested)

fc5f6c7

fixed code style

c6efb2b

removed RLFILTER from default_rotorcraft.xml

e1a40b5

fixed xml file, changed 2.0 to .f for floats, removed typedef + check…

28b7c4a

…ed that it builds

fixed float_mat_transpose --> float_mat_transpose_square and float_ma…

8cef218

…t_transpose_general --> float_mat_transpose

pprz_algebra_float set o[i]=0 at line 717

94726ea

[relative_localization_filter] RL_NUAVS changed from all to just othe…

1ef2e13

…r observed

[relative_localization_filter] ID_array -> id_array

42848de

[decawave_anchorless_communication] added acceleration and yaw rates …

39fba52

…readings to uwb message

[relative_localization_filter] line 91, changed 5 to RL_NUAVS+1 and a…

8f0a9b1

…dded explanation

[relative_localization_filter] added comments to global variables

b0a6853

[relative_localization_filter] moved ekf_rl to c file

f4b3705

[relative_localization_filter] added abi message

6e8bf2c

coppolam added 3 commits May 31, 2018 11:04

[relative_localization_filter] fixed abi message + check build

b994c38

[relative_localization_filter] added line end + added if guard too pp…

6ac4261

…rz message + changed float_mat_identity_scal to float_mat_diag_scal

[relative_localization_filter] updated ABI name + added vz to ABI mes…

2d40650

…sage + added a few comments + tested

coppolam force-pushed the pullrequest_relative_localization_filter branch from 9df6c55 to 2d40650 Compare May 31, 2018 09:07

update pprzlink reference to include RLFILTER message

243bc7a

gautierhattenberger merged commit a733997 into paparazzi:master Jun 10, 2018

coppolam deleted the pullrequest_relative_localization_filter branch June 11, 2018 08:06

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Pullrequest relative localization filter #2262

Pullrequest relative localization filter #2262

coppolam commented May 8, 2018

kirkscheper May 9, 2018

kirkscheper May 9, 2018

kirkscheper May 9, 2018

kirkscheper May 9, 2018

coppolam May 9, 2018

kirkscheper commented May 9, 2018

kirkscheper May 13, 2018

kirkscheper May 13, 2018

kirkscheper May 13, 2018

kirkscheper May 13, 2018

coppolam May 14, 2018

OpenUAS May 30, 2018

OpenUAS May 30, 2018

kirkscheper May 13, 2018

kirkscheper May 13, 2018

kirkscheper May 13, 2018

kirkscheper May 13, 2018

kirkscheper May 13, 2018

kirkscheper May 13, 2018

kirkscheper left a comment

kirkscheper May 14, 2018

kirkscheper May 14, 2018

OpenUAS left a comment

gautierhattenberger May 30, 2018

coppolam May 31, 2018

gautierhattenberger commented Jun 6, 2018


		#include "discrete_ekf.h"

		extern discrete_ekf ekf_rl[RL_NUAVS - 1];

Pullrequest relative localization filter #2262

Pullrequest relative localization filter #2262

Conversation

coppolam commented May 8, 2018

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

kirkscheper commented May 9, 2018

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

Choose a reason for hiding this comment

kirkscheper left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

OpenUAS left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

gautierhattenberger commented Jun 6, 2018