Kalman-Filter, Update position from source other than LOCO-positioning

[flo]

Hello everyone, Hi Arnaud!

As already started in comment to "Loco positioning system" Blog post, I'm doing (or want to do) position control with an "arbitrary" source of position estimation.

In my case I'm using a prototype of an optical flow odometer (4 stereo sensors), which is mounted on the crazyflie using the deck-interface. You can think of it as a 3D optical "mouse".

I have already written a custom deck driver to obtain sensor data via I2C, and to perform normalization (mapping of axes, scaling sensitivity).

Arnaud, thank you for the hint. I had a good look at the source, and I'm now using stateEstimatorEnqueuePosition() which is part of estimator_kalman.c to perform position updates from my deck-driver to kalman-filter.

I already had a look using log-system/plotter so I could see the behavior of the kalman-filter to my "injected" position updates. It is looking good so far - very clean/smooth. Next step will be getting ROS up and running, and then try to set a "known point" to the position controller and see what happens

~Florian

EDIT:
Arnaud, is there a function I can call to update the position-set-point of the position-controller (on crazyflie), and is there a function to set speed, so to say speed-set point if I want to alter the position-set-point by means of moving away from the current one?

I had a look in commander.c and position_controller_pid.c but couldn't find anything appropriate, am I mssing something?

[arnaud]

Hi Florian,

Wow that sounds very cool! Is it an of the shelf sensor?

So far setting the setpoint is only done from the outside, are you looking at setting the setpoint from within the Crazyflie?

For setting the position set point from the ground there is a 'hack' in the commander packet that allows to send a setpoint instead of teleop roll/pitch/yaw/thrust.

If you are using ROS this is handled by the ros example and you should just be able to run the dwm_loc_ekf_hover.launch launchfile as described in the wiki: https://wiki.bitcraze.io/doc:lps:index#ros. I never intended it to be used with something else than the UWB LPS but thinking about it, if you use the internal controller and the kallman filter, it should work fine!

If you instead want to run from python directly I made a quick example to send a sequence of x/y/z setoint: https://wiki.bitcraze.io/doc:lps:index# ... ithout_ros

Setting velocity and position setpoint in a cleaner way is work in progress, there is a couple of tickets on github about a new commander packet and a revamp of the controllers so that more advance control could easily be done.

/Arnaud

[flo]

Hi Arnaud,

it is a prototype of an ASIC (Image-Sensor) + microcontroller circuitry that has been designed to do optical-flow and obstacle avoidance. I need to check if I'm allowed to tell more - I'm lucky to actually have the chance to play with one of the prototypes.

Yes, you are correct. I'd like to alter setpoints within Crazyflie - the purpose is to move away from obstacles.

Actually I'm doing both - I'll set position(fly to coordinate) / speed(manual control) from the ground, and at the same time the crazyflie will also take care of obstacles and alter speed and move away from obstacles if necessary (below safe threshold distance).

Thank you very much for the phyton script, I'll have a look at it as well!

As per today, I have successfully set up ROS on CrazyFlie VM (ROS jade). That might be useful information for wiki, so those who want to run ROS in crazyflie-vm, one can follow your install instructions on Xubuntu 14.04 as well, just replace "kinetic" with "jade".

I launched it the way you suggested (dwm_loc_ekf_hover.launch ). The position of crazyflie is nicely presented in ROS - so odometry seems to work just fine with kalman-filter.

We also tried launching it, but it seems like that we need to do some controller tuning as x,y position is not stable ( we are using a larger platform with brushless motors - total weight is 450gr, total thrust approx. 1.2kg). We will try on stock crazyflie soon. I'll let you know the results!

~Florian

[arnaud]

Hi Florian,

This sounds really cool and I am happy the launch-file worked out of the box. It is not very surprising that you have to tune the position PID for such a bigger platform. Are you using the big-quad deck?

I am very eager to hear about the performance on a stock Crazyflie

Best regards,
Arnaud

[flo]

Hi Arnaud,

I actually forgot to buy bigquad deck.. so I quickly added some pullups to the Crazyflie brushed driver outputs and made some quick change to motor mappings - did work just fine

I'll let you know once I got it running on out of the box Crazyflie. Just need to order more Crazyflies in order to build a stock one (we cut arms to fit it on bigger quad).

Best regards,
Florian