ignore IMU during loco positioning while tracking people

[andrewringler]

I am using the Crazyflie Bolt with Loco Positioning decks and 8x Loco Positioning nodes as anchors in TDoA 3 mode. I am actually getting the position of people (not drones) by mounting the bolt+positioning deck on each person. It seems like the current codebase assumes that the IMU always points up-ish (which is a reasonable assumption for a drone). And the current codebase assumes that the drone faces up and with the correct XY orientation relative to the Anchor layout and is stationary for at least for a few seconds on first power on. When mounted on a person however the bolt can move around a lot orientation-wise. I would like to estimate positions with more focus on the Loco and less focus on the IMU to see if that would improve our positioning estimates.

I did modify the following parameters:

• kalman.pNAcc_xy setting to 100
• kalman.pNAcc_z setting to 100

Which seemed to make positioning much less sensitive to the initial orientation of the Bolt (at startup) and much less sensitive to the orientation of the Bolt during movements. It did not however let me turn the Bolt upside-down; as soon as I turned the Bolt upside down I couldn't really get any kind of accurate positions.

I am wondering if there are any other parameters I should try to modify that would minimize the influence of the IMU on position estimates?

Thanks!

Andrew

[krichardsson]

Hi,
the current kalman filter is designed for flying and is a bit awkward for this type of application even though it probably is possible to tweak it to do what you want. There has been some questions through the years of position only estimation and we have never really tried it out, but finally I thought I'd give it a go - and it turned out be fairly simple. I'm not sure what your requirements are, but maybe it will work for you.

What I did was to add a new estimator, implemented as an Out Of Tree estimator in an Crazyflie app. The estimator only tracks position and reused the measurement model and outlier filter from the kalman estimator. It is very simplistic, it just updates the estimated position with the measured error. The estimator does not use the IMU at all, which means that there is a bit of lag when moving around (it is not for flying), with the current settings in our flight lab it takes a round 1 second to catch up when walking around.
Since we use a static std for the TDoA measurements in the kalaman estimator, I think that this new estimator should be pretty close to the kalman estimator without IMU.

You can find the estimator in the krichardsson/loco-pos-app branch in the firmware repo.

[andrewringler]

This is excellent thank you so much for looking into this. I will post here when I have had a chance to build this and flash on my devices. Do you have a sense for why there is a 1 second lag? I think that with 8 anchors in TDoA 2 mode we get ~62.5 updates per second (1000/16ms), not sure about TWR or TDoA 3, so I was thinking UWB only positioning might be a bit faster. Do you think there are parameters in the estimator we could tweak or is the 1 second lag just having to do with the accuracy of the UWB system when not doing sensor fusion with the IMU? Thanks!

[krichardsson]

The position estimator I wrote is very simple and does not try to predict or use the current velocity or acceleration. When a TDoA sample arrives it simply updates the estimated position by taking a step in the direction that the sample indicates. The size of the step determines how much each sample influences the estimated position, bigger steps means faster tracking but more noise, smaller steps gives more lag but less noise. The step size is controlled by the mixin variable, it is also available as the locopos.mixin parameter.

By the way, tho code for the app is in examples/app_loco_pos and you can read about how to build apps in the documentation