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

Davide_Lentini · March 17, 2022, 1:28pm

Hi everyone , I am trying to calculate the time-delay in ms[milliseconds] of my external navigation system as EKF3 .
To do this I am using a technique. I set the LOG_DISARM parameter to 1 and set an EKF origin , so I can acquire the logs while the drone is unarmed. To save some meaningful values in the logs that make me recognize the accelerometer peaks, I move the drone as fast as possible back and forth a few times with waits between each movement (In such a way as to distinguish the groups of peaks from each other(through a line at the start of the group and at the end of the group)

After doing this, I open the log in the mission planner analyzer.

This is a group of three peaks (i.e. 3 drone movements). The red line represents the accelerometer movements and the green line represents the estimated velocity of North Component’s External Navigation System

What I want to find out is the delay that exists between the peaks of the estimated velocity of external navigation and those of the accelerometer (X axis) in time( I would need this time value in the unit of measurement of milliseconds)

Obviously I expect the peaks of the IMU.AcceX (Accelerometer) to arrive first and then those of the external nav . Mission planner offers the possibility to display the time values on the graph, by pressing: right button and then show values

From what I believe, i have to make a difference between the times of the peaks. In this case following the image below to find the delay between the peak “a2” and “a1” to do delay=(ta2-ta1)[ms], and the same operation for the following peaks
I expect a delay between 20 and 40 ms or anyway constant value even with the other peaks

MY TARGET:

I wanted to get some advice from you regarding the method used to save the log and the program used to view and analyze them. I think Mission planner doesn’t offer much accuracy regarding the time value. Mavexplorer would, I believe, offer better accuracy, but it does not offer a unit of time in milliseconds.

I I need to find a way or a mathematical formula(I had found this formula to use in mavexplorer but I did not understand its use, maybe someone knows it : sqrt( NKF3.IPN^2 + NKF3.IPE^2 ) / GPS.Spd )

To calculate this delay as precisely as possible preferably using mavexplorer or python scripts maybe.
I am also ready to hear advice on the correct method of measurement or your experiences in this field

Thanks to everyone

robertlong13 · March 22, 2022, 2:31pm

If you’re open to using scripts, I’d recommend exporting the the log as a Matlab file (“Create Matlab file” in Mission Planner), and analyzing with Octave or Python (with scipy). I don’t think that’s exactly the answer you’re looking for, but since nobody else replied, I figured I’d throw in my two cents. That’s my go-to if I need to do anything “scientific” with a log…

Also, you’re comparing acceleration and velocity, so you need to compare the time difference between the velocity peak, and the point where the acceleration crosses zero, not where the acceleration peaks.

ppoirier · March 22, 2022, 3:18pm

I generally use an equivalent method but depending on your External Navigation you can compare the 6 DoF pose with the EKF

For example , with an external Visual Odometry system, we compare by looking at VSP vs XF1 for RPY and XYZ

Here is a system I am working with and comparing the Roll it show about 125 msec lag

Davide_Lentini · March 22, 2022, 3:44pm

Thanks you Sir. Yes currently my idea was to convert the .bin log file to a matlab file and use with the scipy library. The idea was to integrate the curve with an integration rule (like Simpson’s) and then compare the curve itself with velocity . Being that I am comparing acceleration and speed, perhaps it is easier to integrate the acceleration to make it similar to speed and to make it easier to identify the delay.
I’m currently looking for a simple guide to integrating using the scipy library and creating plots.

Could you explain to me, in simple words, why do you recommend measuring the delay at the point where the acceleration exceeds the threshold 0? It is not very clear to me

Davide_Lentini · March 22, 2022, 3:53pm

Thanks Sir. Yes , Your method is that’s what I was using initially too. I’m curious, do you use this method frequently? And if you use it frequently, in your opinion, are the delay values reliable?Being that I have to compare the IMU acceleration with the EKF3 speed of an external nav I think the best method is to integrate the acceleration data to compare them better with the speed ones. What do you think? Thank you

robertlong13 · March 22, 2022, 4:29pm

A velocity peak is a point where the velocity goes from increasing to decreasing (or vise-versa). At this same point, by definition, the acceleration goes from positive to negative (or vise-versa), which means it crosses through zero.

Here’s an example of some dummy data I made and plotted. The orange line is the derivative of the blue line. You can see it crosses zero at each peak of the blue line.

Davide_Lentini · March 23, 2022, 10:13am

Hi. I read again your answer . In my system the delay must however be less than 40 ms (Approximately between 20 and 40 ms) . Since Mission planner does not offer the possibility to visualize the delay in such a precise way, I would need to work through programs as “Mavexplorer” or other more precise programs if you know them to analyze mission planner logs . I have already tried using the IMU_FAST option in the parameter list of mission planner to collect data faster but I don’t think this is the right parameter.

My idea was to bypass the parameters that don’t interest me during the log acquisition phase, and acquire only the ones I need such as the IMU.AcceX and the EKF1.VN. Do you know if by chance it is possible to acquire only certain data during the log phase ?

I want to do this to reduce the workload on the FC during log acquisition and focus only on the data I need to analyze.

Do you by any chance know how to apply this setting within the mission planner? Thanks!

ppoirier · March 23, 2022, 11:31am

I would ask @tridge for any recommendation on setting up a precise analysing tool.

Davide_Lentini · March 23, 2022, 11:32am

Thanks , i’ll speak to him @tridge

ppoirier · March 23, 2022, 11:42am

well… we just called … give him some time

tridge · March 24, 2022, 9:50am

I don’t think you’re going to be able to measure this in any meaningful way. The EKF is an inertial estimator. It operates on a delayed time horizon of the slowest sensor (usually the GPS), but it propagates forward to the current time using a buffer of IMU samples, so it has already applied all the IMU samples it has when it outputs a position/velocity estimate. So you can’t use the IMU to measure this lag, as if there is IMU lag it is already incorporated in the output.
There are sources of lag you could look at more meaningfully. For example, some of the IMU datasheets include information about group delays, there is transport lag between the IMU and the MCU over SPI, there is a lag between when we receive the IMU data in the MCU and when we do the EKF update, but all of those lags are going to be quite small.
If you’re interested in a related topic, measuring GPS lag, then take a look at this pymavlink 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

it estimates the lag of the GPS by using the IMU data. I’ve used that to compare the lag in different models of GPS (newer GPS modules tend to have lower lag, a good GPS these days will be 90ms or so, older ones were 200ms)

Davide_Lentini · March 24, 2022, 3:00pm

@tridge Thanks for your response and for the script ! . So if i understand right you wrote that as I read the values between IMU and EKF of ExtNav at the moment, the delay value is not valid .It is not valid because it is affected by too many delays. So I ask you, is it possible to find the raw value necessary for my comparison of the External Navigation? Being that I am using the raw values of the IMU, I therefore wonder if there is, in the logs, a raw value coming from the External Navigation . @ntamas
In the meantime I will investigate the script you sent me, thank you very much

tridge · March 25, 2022, 6:38am

please clarify what you mean by “external navigation”.

Davide_Lentini · March 25, 2022, 9:02am

@tridge What we are trying to do is to fine tune an indoor localization system based on UWB technology. The tags present on the drones independently calculate the position and pass it to Ardupilot as "External nav"in EKF3. We need now to calculate the delay of this Externalnav and set this delay inside the Ardupilot code. Thank you

tridge · March 25, 2022, 9:39am

ok, thanks. That sounds more possible.
What exact messages do you pass into ArduPilot? Do you have a log you could share?

tridge · March 25, 2022, 9:44am

note that you need a log with significant lateral acceleration. Flying a figure-8 for example.

tridge · March 25, 2022, 9:45am

also, please set LOG_DISARMED=1 and LOG_REPLAY=1. Having a replay log will allow us to optimise the delay via replay playback

Davide_Lentini · March 25, 2022, 10:12am

Hi @Tridge . Thanks for your response , here is the log that I acquired with the LOG_DISARMED already at 1, very soon I will send you the one with the LOG_REPLAY at 1:

1 01-01-1980 01-00-00.bin (676 KB)

Our tag outputs only the X, Y, Z position and no velocity data to
the external nav as you can see from the parameters below:

Currently with the log attached my “method” to calculate the delay is the one described in the image below. Measure the temporal values of the peaks with the help of the mission planner tools:

In the image above my method is to calculate the difference in [ms] between peak “a2” and peak “a1”
delay=ta2-ta1
There is probably a more precise method that you are familiar with. Indeed without the help of velocity from the tag, the script ,that you shared a few posts ago, becomes unusable, can you confirm it?

I would also like to quote you this part of my previous post:

In my system the delay must however be less than 40 ms (Approximately between 20 and 40 ms) . Since Mission planner does not offer the possibility to visualize the delay in such a precise way, I would need to work through programs as “Mavexplorer” or other more precise programs if you know them to analyze mission planner logs . I have already tried using the IMU_FAST option in the parameter list of mission planner to collect data faster but I don’t think this is the right parameter.

Thanks , Davide

Davide_Lentini · March 25, 2022, 2:20pm

Hi @tridge ! So here i attach the function we use to update the state of the sensor ( More precisely where the position data is updated )

AP_Beacon_TDoA-update_function.cpp (3.0 KB)

this->send_externalNav(position_ned, position_error, pkt0_time, estimated_position);

const uint32_t delay_ms = 0; // TODO average delay of external nav system measurements relative to inertial measurements

AP::ahrs().writeExtNavData(position_ned,
attitude,
posErr,
angErr,
timestamp_ms,
delay_ms,
reset_timestamp_ms);

}

I believe that these lines of code answers your question :

What exact messages do you pass into ArduPilot? Do you have a log you could share?

Thanks @tridge

tridge · March 27, 2022, 7:52pm

I had a quick look at the log, and the largest innovations are in height.
A replay log may help us find a better delay value. We’d have to setup the delay as an EK3 parameter so it can be adjusted in replay runs.
Note that the vibrations may make the measurement difficult. Please use a soft tune so the flight is as smooth as possible