how to get 3D position of CF according to twr algorithm

All discussions related to the Loco Positioning system
Post Reply
nanchen
Beginner
Posts: 1
Joined: Mon Jun 19, 2017 2:40 pm

how to get 3D position of CF according to twr algorithm

Post by nanchen »

hi,arnaud

1. I am studying the LPS code of crazyflie. I know the twr algorithm only get the distance between CF and anchor. I don't understand how to get position of CF only based on the distance. Is stateEstimatorUpdateWithDistance() the function getting the position? is there any Reference material or formula about these code.
2. I want add some debug message about the position of CF, I should printf the S[STATE_X], S[STATE_Y],S[STATE_Z], right?

Thanks
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: how to get 3D position of CF according to twr algorithm

Post by arnaud »

Hi,

1. The position is calculated by the Kalman filter, look in the kalman filter it is based on a couple of papers mostly form ETH Zurich: https://github.com/bitcraze/crazyflie-f ... r_kalman.c

2. You can print these on the console yes. You could also use the log subsystem to log and record the x/y/z state on the ground.
Post Reply