Delayed Kalman navigation based on two way ranges

I am interested to know the basic principle of the "Delayed KF" when considering an underwater robot aiming to localize it self using a LBL system.

More practically, in order to calculate the distance to each anchor, the robot transmits a PING to and waits for the RESPONSE from the anchor. This is time consuming as the sound velocity is very low (1500m/s). Lets say it will take order of 5/10 seconds to complete such iteration (large distances).

If the robot is static (during this operation, or the anchors are very close), I can recover the two travel time, TWTT, and after that the distance (knowing the sound velocity, divide by two etc). In that case i can include the range to my EKF navigation filter.

Assuming that the robots moves (1 m/s) this assumption does not hold anymore as I can not divide the TWTT by a factor of 2.

Searching online i found talking about the delayed EKF in which (in my understanding) having a measurements timestamped (the real time and the time when i got it) i re-run the filter at the right time when the measurements was/or should have been done.

Does this hold also for this case and how can be integrated with the navigation filter of the robot?

Geometrically to find the right distance based on the TWTT and considering the movement done during that time seems hard to me to model. At least i have not found any paper in the literature.

Regards,

• I guess you don't have any odometry measurement, not even inaccurate ones ? Feb 13 '18 at 1:22
• Hi Malcom, yes I have some data such as velocity and heading from the robot. Of course not so accurate. This is the basic case. Can this case been handled? Thank u so much
– john
Feb 13 '18 at 10:05
• I don't really know, I'm no expert on that. But it seems like an delayed EKF deals with delayed measurements, while here, it seems to me that you don't have so much delayed measurement as inaccurate measurements. The ping you receive does not contain any data such as timestamps or other ? It's just a ping ? Feb 13 '18 at 18:22

I believe you're not taking the problem with the right angle in mind.

Your ping measurements are not delayed measurements, they are wrong. The problem is not that you are receiving them late but that, by receiving them late, you lost the actual information that they were providing you. That is because you are calculating the position this way : Image shamelessly taken from this paper by Rui Almeida

But this relationship is not 100% correct when you are moving. You could try to model this error as either a bias or a Gaussian noise but, since the error is due to the vessel movement, I believe your system is not suffering from a Gaussian noise or bias, and that would not be very effective.

From my point of view, you would have a better chance by using an EKF on the odometry model of your robot and using it to find the right TTWT from there. But i'm not a Kalman filter wizard at all so I hope someone will pass by and prove me wrong.

• Dear Malcom! Thank you very much for your promt response and you effort. You are completely right! It is not only that the measurements are delayed but also wrong. For me it is hard to imagine e correct solution even if I use the data of the robot. This either from a EKF point of view (which i am a beginner) and/or a geometrical solution. Lets hope some-one else give a definitive answer :)
– john
Feb 14 '18 at 18:27

I will answer with respect to the unscented kalman filter (ukf) instead of the extended kalman filter (ekf) because I think the ukf is easier to understand. You can either switch to using ukf yourself (as I have) or translate the idea back into ekf. They are similar enough that the same idea would work.

In ukf the probability distribution of your state space is represented by a swarm of "sigma points." When receiving a new measurement, we need to translate each of the sigma points into measurement space so that so that they can be compared with the actual measurement.

Let's say your state includes variables x, y, vx, and vy to represent both position and velocity. Each sigma point would have these same variables. The anchor has position anx, any. The main challenge is to calculate the expected return time (rt) of a sonar ping that reaches you now and starts at your previous position at rt time ago. If you can do that for each individual sigma point, then combining those results into an overall state update is just following the usual ekf recipe. We'll assume that velocity was constant during the period of time that this measurement took place.

Current position: x, y

Previous position: px = x - vx * rt, py = y - vy * rt

Expected rt: rt = ((distance from x,y to anx,any) + (distance from px,py to anx,any)) / (speed of sound)

Ideallly you would treat these as a system of equations with rt completely unknown, but that's probably more trouble than it's worth. You can get away with using the measured value of rt for estimating the previous position part. The rt that you calculate in that last equation is what you'll compare with the measured rt.