UVDAR measurements affected by high error

[Kaktusowy500]

Hello,
When I was testing an UVDAR System in simualtion, I found a quite strange problem. Major and unexpected shift in measured position occurs when I change observer heading a bit and observed drone remains stationary. In some situations it works pretty well but in another the error is really high. During my investigation I found that this issue occurs when drones are in distance grater than 7-8 meters and it repeats about every 15 degrees change in heading. For test, I spawned two quadrocopters in 10m distance along x axis, on the same altitude and then I was changing heading of observer very slowly. Example situation:

Changes in measured position on each axis, compared to ground truth are presented bellow:

I also prepared XY plot presenting a total error in position (euclidean distance between measured and ground truth position, in meters) in relation to observer heading(degrees). It was made when the position between drones was equal about 10 meters.

Issue is rather not related to observer movement because it also occurs when observer remains stationary but has the specified affected heading. I have found that error is lower when the drone is visible on two camera (heading close to 0). Problem does not occur when observed drone is rotated in such a manner that its 3 LEDs are visible.

What could be causing this issue and is there any way to eliminate or mitigate it? Is it a known constraint of the system or maybe my configuration is incorrect?

Thanks a lot for your help!
Regards

Detailed steps to reproduce

Start from working mrs system and freshly cloned uvdar_core package then add or modify following files of in uvdar_core package:

Add tmux script to scripts directory:

`uvdar_single_uav.yml`:

name: simulation
root: ./
startup_window: uv_observer 
pre_window: 'export RUN_TYPE=simulation; 
            export UAV_TYPE=f450; 
            export WORLD_NAME=simulation; 
            export ODOMETRY_TYPE=gps; 
            export SENSORS="garmin_down"
            export WORLD_FILE=$(rospack find uvdar_core)/scripts/custom_configs/world_simulation.yaml'
windows:
  - roscore:
      layout: even-vertical
      panes:
        - 'pkill px4;
          roscore'
  - gazebo:
      layout: even-vertical
      panes:
        - waitForRos; roslaunch uvdar_core uvdar_gazebo.launch gui:=true
  - status:
      layout: even-vertical
      panes:
        - export UAV_NAME=uav1; waitForRos; roslaunch mrs_uav_status status.launch
        - export UAV_NAME=uav2; waitForRos; roslaunch mrs_uav_status status.launch
  - spawn:
      layout: even-vertical
      panes:
        - waitForSimulation; rosservice call /mrs_drone_spawner/spawn  "1 $UAV_TYPE --pos-file $(rospack find uvdar_core)/scripts/positions.csv --enable-rangefinder --enable-ground-truth --enable-dual-uv-cameras --enable-back-uv-camera --uvcam_calib_file \"$(rospack find uvdar_core)/include/OCamCalib/config/calib_results_bf_uv_fe.txt\""
        - waitForSimulation; sleep 12; rosservice call /mrs_drone_spawner/spawn  "2 $UAV_TYPE --pos-file $(rospack find uvdar_core)/scripts/positions.csv --enable-rangefinder --enable-ground-truth --enable-uv-leds --uvled_s_l 0 --uvled_s_r 0"
  - control:
      layout: even-vertical
      panes:
        - export UAV_NAME=uav1; waitForOdometry; roslaunch mrs_uav_general core.launch config_mpc_tracker:=./custom_configs/mpc_tracker.yaml
        - export UAV_NAME=uav2; waitForOdometry; roslaunch mrs_uav_general core.launch config_mpc_tracker:=./custom_configs/mpc_tracker.yaml
  - autostart:
      layout: even-vertical
      panes:
        - export UAV_NAME=uav1; waitForSimulation; roslaunch mrs_uav_general automatic_start.launch
        - export UAV_NAME=uav2; waitForSimulation; roslaunch mrs_uav_general automatic_start.launch
  - takeoff:
      layout: even-vertical
      panes:
        - 'export UAV_NAME=uav1;
          waitForControl;
          rosservice call /$UAV_NAME/mavros/cmd/arming 1;
          rosservice call /$UAV_NAME/mavros/set_mode 0 offboard'
        - 'export UAV_NAME=uav2;
          waitForControl;
          rosservice call /$UAV_NAME/mavros/cmd/arming 1;
          rosservice call /$UAV_NAME/mavros/set_mode 0 offboard'
        # #}
  - rviz:
      layout: even-vertical
      panes:
        - export UAV_NAME=uav1; waitForControl; roslaunch uvdar_core multimodal_hypothesis_rviz.launch
  - uv_observer:
      layout: even-vertical
      panes:
        - waitForCompile; gz world --pause=0; sleep 6;  export UAV_NAME=uav1; waitForControl; roslaunch uvdar_core sim_three_sided.launch
  - kalman_filter:
      layout: even-vertical
      panes:
        - export UAV_NAME=uav1; waitForControl; roslaunch uvdar_core uvdar_kalman_identified_3.launch output_frame:=$UAV_NAME/stable_origin

uvdar_single_uav.sh:

#!/bin/bash

if_is_link=`readlink $0`
command_name=$0
if [[ if_is_link ]]; then
  command_name=$if_is_link
fi

MY_PATH=`dirname "$if_is_link"`
MY_PATH=`( cd "$MY_PATH" && pwd )`
cd $MY_PATH

# remove the old link
rm .tmuxinator.yml

# link the session file to .tmuxinator.yml
ln uvdar_single_uav.yml .tmuxinator.yml

# start tmuxinator
tmuxinator

Add positions.csv file to scripts/custom_configs directory

``` 1,0.0,0.0,2.0,0.0 2,10.0,0.0,2.0,0.0 ```

Add custom_config/world_simulation.yaml:

#
## DEFINITION OF THE ARENA
#

utm_origin_units: 1 # 0 - UTM, 1 - Latitude Longitude
# !!! BEWARE !!!: the lat&lon and UTM coordinates below might be wrong
# !!! do NOT just switch utm_origin_units from 0<->1, the other form
# !!! might not be updated or even set correctly in the first place.
utm_origin_x: 465710.758973
utm_origin_y: 5249465.43086
utm_origin_lat: 47.397743
utm_origin_lon: 8.545594

init_gps_origin_local: false # origin of gps_origin will be in the position of UAV at odometry launch
init_gps_offset_x: 0.0 # initial position of UAV relative to gps_origin when init_gps_origin_local is enabled
init_gps_offset_y: 0.0 # initial position of UAV relative to gps_origin when init_gps_origin_local is enabled

safety_area:

  use_safety_area: false

  frame_name: "gps_origin"

  # convex polygon CCW
  safety_area: [-50, -50,
  50, -50,
  50, 50,
  -50, 50]

  polygon_obstacles:
    # loaded as a vector of matrices
    # each matrix has polygon vertices in columns
    # [[M1], [M2]]
    enabled: false
    data: [8.6, 18.61, 20.35, 6.82, -3.14, 10.7,
           -22.5, -22.71, 1.02, 17.68, 11.1, -4.1]
    rows: 2 # each matrix has two rows
    cols: [6] # nums of cols of each matrix

  point_obstacles:
    # loaded as a vector of matrices
    # x, y, radius
    enabled: false
    # [x1, y1, radisu1, height1,
    #  x2, y2, radius2, height2]
    data: [-5.0, -5.0, 2, 2,
           -10.0, -10.0, 4, 2]
    rows: 1 # each matrix has a single row
    cols: [4, 4] # nums of cols of each matrix

  max_height: 15.0
  min_height: 0.5

replace mpc_tracker.yaml content with:

version: "1.0.2.0"

mpc_rate: 100.0 # rate of MPC calculation, >= 10 Hz

diagnostics: # diagnostics publisher
  rate: 30                             # [Hz]
  position_tracking_threshold: 1.0     # [m] distance considered as "in place"
  orientation_tracking_threshold: 0.3  # [rad] orientation error considered as fine during tracking

braking:
  enabled: true # true
  q_vel_braking: 2000.0
  q_vel_no_braking: 0.0

wiggle:
  enabled: false
  amplitude: 0.5 # [m]
  frequency: 0.2 # [Hz]

# mrs collision avoidance
collision_avoidance:

  enabled: false # true
  trajectory_timeout: 1.0 # [s]
  radius: 0 # 3.0 # [m]
  inflation_radius: 0.6 # 1.0 # [m] if a collision is detected in (radius + inflation radius) the uav will start to slow down. Do not set this variable lower than 0.5
  altitude_threshold: 2.9 # [m]
  correction: 0.1 # 3.0 # [m]
  predicted_trajectory_publish_rate: 2 # [Hz]
  collision_horizontal_speed_coef: 0.15 # when collision detected, slow down during the manouver
  collision_slow_down_fully: 4 # 10 # when collision detected, slow down fully this number of steps before it
  collision_slow_down_start: 4 # 25 # when collision detected, start slowing down this number of steps before it
  collision_start_climbing: 4 # 25 # when avoiding, start climbing this number of steps before it

model:

  translation:

    n_states: 12
    n_inputs: 3

    A: [1, 0.01, 0.00005,       0, 0,   0,        0,       0, 0,    0,       0,       0,
        0,    1,    0.01, 0.00005, 0,   0,        0,       0, 0,    0,       0,       0,
        0,    0,       1,    0.01, 0,   0,        0,       0, 0,    0,       0,       0,
        0,    0,       0,       1, 0,   0,        0,       0, 0,    0,       0,       0,
        0,    0,       0,       0, 1, 0.01, 0.00005,       0, 0,    0,       0,       0,
        0,    0,       0,       0, 0,    1,    0.01, 0.00005, 0,    0,       0,       0,
        0,    0,       0,       0, 0,    0,       1,    0.01, 0,    0,       0,       0,
        0,    0,       0,       0, 0,    0,       0,       1, 0,    0,       0,       0,
        0,    0,       0,       0, 0,    0,       0,       0, 1, 0.01, 0.00005,       0,
        0,    0,       0,       0, 0,    0,       0,       0, 0,    1,    0.01, 0.00005,
        0,    0,       0,       0, 0,    0,       0,       0, 0,    0,       1,    0.01,
        0,    0,       0,       0, 0,    0,       0,       0, 0,    0,       0,       1]

    B: [0, 0, 0,
        0, 0, 0,
        0, 0, 0,
        0.01, 0, 0,
        0, 0, 0,
        0, 0, 0,
        0, 0, 0,
        0, 0.01, 0,
        0, 0, 0,
        0, 0, 0,
        0, 0, 0,
        0, 0, 0.01]

  heading:

    n_states: 4
    n_inputs: 1

    A: [1, 0.01, 0.00005,       0,
        0,    1,    0.01, 0.00005,
        0,    0,       1,    0.01,
        0,    0,       0,       1]

    B: [0,
        0,
        0,
        0.01]

mpc_solver:

  horizon_len: 40 # Horizon length is hardcoded in solver code, this value is used in other parts of the code

  # dt1: 0.01 # dt1 is set as 1/main_rate
  dt2: 0.2

  xy:
    verbose: false
    max_n_iterations: 25 # default: 25
    Q: [5000, 0, 0, 0]

  z:
    verbose: false
    max_n_iterations: 25 # default: 25
    Q: [5000, 0, 0, 0]

  heading:
    verbose: false
    max_n_iterations: 25 # default: 25
    Q: [5000, 0, 0, 0]

Add turn_around_test.sh script which changes observer heading:

#! /bin/bash

while true
do 
    rosservice call uav1/control_manager/set_heading_relative "goal: 0.05"
    sleep 0.1
done

Run ./uvdar_single_uav.sh

When drones had started and are hovering run ./turn_around_test.sh

Use Plotjuggler for visualization

[klaxalk]

This is probably a question for @ViktorWalter.

[ViktorWalter]

This is the result of the geometry of the target image - the visible LEDs - providing less precise information when only two are seen.
In essence, if only two LEDs are seen, this observation can correspond to greater range of relative poses - combined relative orientation and relative position - than if three are seen.
Imagine that you are rotating the target withing the range of relative orientations where only two LEDs are seen.
Their mutual distance in the image is combination of the distance of the whole target and its relative angle w.r.t. the observer, and this relation is coupled - there is ambiguity as to which of the two parameters has what influence.
The relative distance you are receiving is the mean of this range for distance, which also explains why it jumps when you switch between the two cases.
This is one of the reasons why I'm always providing measurement error covariance (if you are not familiar with it, it is explained intuitively here) with the output. This covariance approximately covers the statistical range around the reported mean in which the true relative pose can be.
The mean itself changes between the two cases - two or three LEDs visible - but the ranges should be compatible.
If you want to see this better, you may plot the target distance (norm of relative position) expanded by e.g. 2-3 sigma (standard deviation) which you gain by rotating the measurement covariance such that the direction towards the target becomes X, and then you extract the top-left element as square of the sigma of range measurement.
You should - and this is in general good practice in robotics - never base your behavior purely on the mean of the sensory measurements, but you should take the known current precision into account as well.
You can use the measurement error covariance information within e.g. a Kalman filter, or to reduce the intensity of vision-induced actions such that you don't react to the errors.

[Kaktusowy500]

Thanks for your reply!
I will try to make use of error covariance. You mentioned that it may be used within Kalman filter. Am I right that measurement error covariance is already used by UVDARKalman node but covariance matrix, which is published in filteredPoses, may be exploited in further processing?
I am also wondering if adding one more LED, few centimeters above the uav's frame would result in better precision. This LED could be visible from all directions, and provide additional information (similar to the current situation when 3 LEDs are seen). I am aware that this modification will require some changes in algorithm and could cause other issues but it just an idea for improvement. Does it make sense or have you thought about something similar?

[ViktorWalter]

Yeah, the filteredPoses output is already processed with Kalman filter. The raw inputs into it are from measuredPoses#. You can still use the filtered output in your own Kalman filter in order to e.g. tweak the dynamics of the mean to better suit your own purposes or to fuse it with another data source.
As for the additional LED suggestion - we did actually briefly try this option. You can still see traces of it in the code, under the keyword beacon. It turns out that it is not practical - for one it requires building a "tower" with multiple LEDs (for all directions) above the center of the UAV, where it obstructs other equipment. Additionally, from lower angles - if the beacon was not significantly elevated above the UAV - the beacon tended to be visually quite close to the other LEDs, which led to their signals mixing up. This issue is not typically present in the layout with LEDs on the perifery of the UAV, since they radiate sideways with less intensity than forwards (Lambertian pattern).
If, however, it is not an issue for you to build a tall beacon tower on your UAV, you may do it - look into uvdar_core/config/models/quadrotor_beacon.txt. I've updated the file now (since it wasn't reflective of our current system) for an example of the layout. You have to set custom_model in the launcher to true and model_file to refer to uvdar_core/config/models/quadrotor_beacon.txt. You can then spawn drone with the parameter --enable_uv_leds_beacon and set its signal ID with --uvled_beacon_f # to one greater than the signal IDs of the rest. This wasn't tested for a longer time, so it may be buggy.

[Kaktusowy500]

Thanks a lot, I will gladly take a look at this solution