Working code for single basestation

[NicksonYap]

Hi,

I've managed to implement the formula in MATLAB into CF, turns out there are some heavy calculations required to solve the matrix equation (involving SVD & pseudoinverse) but was turned into a simple function in MATLAB.

The algorithm works by gathering all possible Ray vectors (max 4 sensors * 2 BS = 8 rays)
Then get all the possible pairs of rays, calculate the center and average them
(max only using 12 pairs due to performance issue)
This will will work no matter the combination of sensors or basestations, as long as a pair of rays can be retrieved.
It requires the Rotation Matrix and can only accept 2 rays per position calculation.
(I've concluded that any trying to get Position and Pose using >2 rays will require some kind of solver)

Solves: #461
The code used here is quire heavy, but it works so it should be consided as beta.
The ideal implementation could be a direct port from of the most minimalisic implementation from libsurvive

Note that when switching reception between different basestations or between single to dual basestation, the estimated position may shift (5-10cm for my case), but kalman filter will try to reduce the impact.
Reasons:

1. Tighter calibrations needed,
2. bring the BS closer to each other
3. reduce distance between BS and CF
4. increase distance between sensors

It should also slightly improve reception and accuracy when using 2 BS (allows different sensor to receive from different BS, and averages up to 12 times)

Try this at your own risk!

:)

[NicksonYap]

I'm not sure if a squash of all commits is needed, let me know if so

[NicksonYap]

Forgot to mention that the basestation positions need to be converted to with respect to CF
Had just added python code for that

[krichardsson]

I did a quick test of your code today.

It seems to work partially in our lab, when we block the slave base station the position seems to be OK but if we block the master base station the position moves away about 3 meters. Not sure why.
We had some loss of radio communication from time to time that I did not have time to investigate. A theory is that it could be related to CPU starvation.
We could also see a small step in the estimated position when blocking the slave base station. I assume this is what we discussed in issue #461 when going from intersecting beams to one base station only.

Even though everything is not working, it seems as you are on to something.

How do you want to proceed with this?

[NicksonYap]

The development and initial testing was done in a 5x3.5m space, which seems to work well when blocking any BS (except for the 5-15cm shift when switching)

I noticed a similar issue with position when I tried it in a large space (10x4m)
Similarly for my case, blocking the slave works very well (except when too far away from the master BS)
Blocking the master causes a large shift at certain areas, and snaps back to position at other areas.
In fact, the Z-axis seems to flip (going up shows the CF going down)
I suspect there is some calculation issue (some sort of overflow perhaps)
Probably the 5-15cm shift I had was related to this.

I'll need some time to debug at the larger area as the issue seems more obvious.

How far apart are your BS in your setup / how large is the space?

I hadn't had any issue with radio connectivity, but I had issues with position readings spiking at fixed intervals. Couldn't it be related?
I do agree there may be CPU starvation, I'll need to optimize the code further as it calculates a new position pretty much at every sweep

[krichardsson]

Hi @NicksonYap ,
Thanks for all your work!

i was looking a bit at this again and this PR is getting pretty large :-)
I would like to possibly pull this in smaller chunks, not sure how to do it though as I want to keep you as the contributor but make some modifications.
Do you have some sort of documentation of your algorithm that I can start from?

[NicksonYap]

@krichardsson I'll combine/squash the commits into smaller chunks before actual merging of the PR
Just hope that the constant commits in my branch wont send annoying notifications to you guys

I've fixed several issues and increased the reliability in the smaller 5x3.5m space (doesn't jump abruptly when switching BS),
but still hadn't tested at the 10x4m space yet

Regarding documentation, I had actually derived the formulas myself, not sure if i'd want to show step by step, but I do have a MATLAB file to visualize, test scenarios and prove that the math is valid (will share when going to merge)

Regarding the issue you had initially when testing the repo, can you send me your BS geometries/matrices? This way I can check if they are correct by visualizing in MATLAB, and know exactly how large the space is,
Just in case there is a bug for a particularly large space

[krichardsson]

@NicksonYap We have chosen to write our own implementation, see #461. Thanks for your work none the less!