Calculate the delay of the External Navigation System as EKF3 inside the log

Davide_Lentini · March 29, 2022, 12:54pm

Hi @Tridge , Thanks for taking your time to look at the log. Thanks for the advice to use LOG_REPLAY and SOFT_TUNE . When you wrote :

We’d have to setup the delay as an EK3 parameter so it can be adjusted in replay runs.

Are you talking about adding a parameter to Mission Planner’s parameter list which will make it easier to modifiy the delay of the external navigation and visualized inside the replay log , right ?

Thanks, Davide

Davide_Lentini · October 22, 2022, 2:23pm

Hello ! @tridge

After some time I returned to this project being that there has been quite a bit of progress .

First of all I managed to save in the logs a custom parameter (LBPR) that saves the actual external navigation data (PosX(m),PosY(m),PosZ(m)) referred to our Indoor system ( the indoor positioning results are very good , so I am confident of the data saved in the logs ) . Using this high freq method :
https://ardupilot.org/dev/docs/code-overview-adding-a-new-log-message.html#the-harder-way

Our goal now is to find the delay in milliseconds between our External Navigation system and the accelerometer data .

I have reread the comments in this topic and in these days I have been trying to acquire logs that would be satisfactory for the delay calculation .

Based on this advice , I enabled the LOG_DISARMED parameter , and made the drone make very fast movements in the positive X-axis .

I then analyzed the logs , and by analyzing the PosX of the LBRP custom parameter and the AccX of the IMU I saw that the peaks of change occurred almost simultaneously ( that “almost” I think depends on the delay ) .

This is the acquired log .

The name of the custom parameter ,which contains 3 data items ( PosX,PosY,PosZ) that refer to the raw values of the positions from the External Navigation we use, it is LBPR .

Comparing LBPR_PosX with IMU_AccX , we can see that the peaks of movement on the positive X-axis are common .

Now I need help to find the delay (ms) between the ExternalNavigation system and The accelerometer .
I am trying to edit the mavgpslag.py file to replace the GPS data with that of my custom LBPR parameter .

Since our External Navigation system does not return a velocity in m/s , we need to derive it from the PosX,PosY,PosZ data it provides , and then compare the external navigation velocity curve with the accelerometer integration curve .

I am having problems in the section to derive the velocity of my new LBPR parameter , there is something wrong with my code .

mavgekf(gps)lag.zip (2.1 KB)

I do not want to go into too much detail in this first post, because I want to wait first for an opinion from an expert and figure out the best way to go about getting this delay value .

However, the idea is to figure out if the attached log is accetapible to get the delay, and the best method to get it

Thanks all !

Davide_Lentini · March 24, 2023, 11:23am

@tridge @ppoirier
Hello everyone ! I wanted to thank you for your patience with this post .
My work to calculate the delay of an Ekf system in correspondence with IMU values , has been going on these months , among many difficulties , since this is not my field and it is not a certain subject that can be learned overnight ; Let alone solve such a problem !

Current situation and progress made :
-3d Map :
Our UWB system has also been very well received graphically . As I say in the previous post , through the custom High Frequency log that I added , I was able to create a python script that would analyze the log saved by the drone and create a three-dimensional (interactive) map , which displays both the coordinates of the UWB anchors , and the path taken by the drone during the acquisition of the log , just thanks to the info : X,Y,Z . Here is an image of the map :

-Delay Calculation:
Also on this side there have been important updates and progress , but not such as to lead to the completion of the goal , so I ask for your help again , to be able to complete this project of mine , I thank you !

So , I summarize my method to acquire the logs useful for delay calculation ;
I simply move the drone back and forth on the X axis of the UWB system (with LOG_DISARMED at 1 of course and LOG_BITMASK with Fast IMU active).

Initially I was getting logs that in my opinion were too far from my idea of delay ; The peaks of the PosX (UWB) were in fact very misaligned compared to the peaks of the AcceX. After a long time , going crazy trying to figure out if it was a UWB problem , hardware , software or something else , I came to the conclusion that the high frequency log , had a bug in the code and was being picked up later than the IMU log . Here from the picture you can see the difference :

On the left , the graph BEFORE the correction of the bug; on the right the graph AFTER the bug fix.
As you can see in the graph on the left, the red peak (PosX), which represents the moment when the drone is moved forward for the UWB system, was too far ahead (or behind) the peak of the AcceX (in green). I thought it could be the delay but I was dubious.
On the right instead , after the bug fix , it is evident that the peaks are practically overlapping , which makes me think that the delay value is hidden somewhere in that very short moment and that it is a very short ms value .And that the code and method are the correct ones , maybe not the best ones , but at least the correct ones
In this other image you can see at 46 seconds , thanks to the graphic line created by Mission Planner , the difference between the PosX peak and the AcceX peak :

The analyzed log is this :

Please , if you can , analyze it and tell me what you think about it , thanks !

My last attempt to calculate the delay , was to use an existing script :

github.com

ArduPilot/pymavlink/blob/master/tools/mavgpslag.py

#!/usr/bin/env python

'''
calculate GPS lag from DF log

The DF log file needs to be generated with ATT, GPS and IMU bits:
-  On copter (bit0, bit2 and bit18) set on the LOG_BITMASK parameters
-  On rover  (bit0, bit2 and bit7) set on the LOG_BITMASK parameters
-  On plane  (bit0, bit2 and bit7) set on the LOG_BITMASK parameters
Make sure IMU_RAW (bit 19) is not set

For this to work, the vehicle must move at speeds above the --minspeed parameter (defaults to 6m/s)
The code really only works when there is significant acceleration as well.
You'll need to fly quite aggressively on a copter to get a result.

'''
from __future__ import print_function
from builtins import range

import os

This file has been truncated. show original

Modified appropriately , after initial problems due again to the bug I identified and fixed , it was able to read the log successfully without any error . And it prints a value , namely 44 ms . It seems to me that these values are directly proportional to the size of the log and more precisely to how many position changes (spikes) I make the drone do during the log .
For example with 35 spikes the script prints 44 ms .
If I make 25 it comes out about 36 ms ,
If I make 55 it comes out about 60 ms .
These seem to me not useful values , and I am not sure why the script behaves this way . Please if you have useful information or have time to help me , tell me and I will add you to the Github repository that contains the script , so that you can analyze it easier .

This concludes my discussion on delay , if you have any doubts , useful answers , please post your ideas here and I would be very grateful . You are the experts here and I rely on you