I'm writing some Quad Copter software and beginning to implement an altitude hold mode.

To enable me to do this I need to get an accurate reading for vertical velocity. I plan to use a Kalman filter for this but first I need to ensure that I'm getting the correct velocity from each individual sensor.

I have done this but I'm not 100% sure its correct so I was hoping to get some confirmation on here.

My first sensor is a Lidar distance sensor, I calculated acceleration and velocity using the following code:

float LidarLitePwm::getDisplacement()
    int currentAltitude = read();
    float displacement = currentAltitude - _oldAltitude;
    _oldAltitude = currentAltitude;

    return displacement; //cm

//Time since last update
float time = (1.0 / ((float)FLIGHT_CONTROLLER_FREQUENCY / 10.00)); // 50Hz, 0.02s

float lidarDisplacement = _lidar->getDisplacement();
_currentLidarVelocity = lidarDisplacement / time;

The second sensor is an accelerometer. I calculated acceleration and velocity using the following code:

Imu::Acceleration Imu::getAcceleration()
    //Get quaternion
    float q[4];

    //Get raw data
    float values[9];

    //Extract accelerometer data
    float acc[3];
    acc[0]= values[0]; //x
    acc[1]= values[1]; //y
    acc[2]= values[2]; //z

    //Gravity compensate
    _freeImu.gravityCompensateAcc(acc, q);

    //Convert acceleration from G to cm/s/s
    _acceleration.x = acc[0] * 9.8 * 100;
    _acceleration.y = acc[1] * 9.8 * 100;
    _acceleration.z = acc[1] * 9.8 * 100;

    return _acceleration; //cm/s/s

//Time since last update
float time = (1.0 / ((float)FLIGHT_CONTROLLER_FREQUENCY / 10.00)); // 50Hz, 0.02s

//Get accel
Imu::Acceleration imuAcceleration = _imu->getAcceleration();

//Get velocity
currentZVelocity += imuAcceleration.z * time; //cm/s

It would be great if someone could confirm if this is correct (or not)

Thanks Joe

  • 1
    $\begingroup$ Just out of curiosity, why do you need these values? If you want to hold altitude you'd only need to know the distance to the ground. I'm pretty sure you could build a PID controller that tries to hold d constant without specifically knowing your v or a. Or rather, those values would be implicitly calculated in the PID as a result of the tuning algorithm. $\endgroup$
    – Octopus
    Mar 25, 2015 at 17:17
  • $\begingroup$ If I only wanted to hold an attitude then yeah it works OK with only one PID although it oscillates up/down about 30cm and I can't get it tuned out. However I want to be able to move up and down at a certain velocity, this means I need two sets of PID loops, one PID for altitude and one for velocity. Then you override the altitude PID outputs with a vertical velocity from the operator and feed this into the velocity PID's when the quadcopter should be changing altitude. $\endgroup$ Mar 25, 2015 at 17:27
  • 1
    $\begingroup$ If your PID for keeping altitude does oscillate, then don't expect to get better results with PID's working with speed and acceleration feedback. That being said, you can still use just the first PID by properly varying the altitude reference in a tracking setting: a varying altitude set-point incorporates the notion of velocity so as acceleration. Then, if the PID is tuned for tracking, your quad will follow position, velocity and - to a certain extent acceleration - as well, all at once. $\endgroup$ Mar 26, 2015 at 7:19
  • $\begingroup$ Further, the way you compute velocity and acceleration is a bit naive and you'll end up with very noisy estimates of those physical quantities. Either you've got to move to more sophisticated techniques (Kalman, SG filter ...) or - again - go back to the suggestions above. $\endgroup$ Mar 26, 2015 at 7:20
  • $\begingroup$ Hi Ugo, I was hoping having two PID's would improve things as that is how I did my stabilisation PID's (one for angular rate of change and one for angle), this would be set up the same way (one pid for vertical rate of change and one for altitude). Another reason is that I want to do auto take off and landing and I found that when I enable my altitude PID to take off it does not work well and very suddenly increases the throttle because the PID output is biased at 0.5 to make it oscillate around hovering power. $\endgroup$ Mar 26, 2015 at 8:20

1 Answer 1


I noticed a weird thing in your code -- It looks like you are calculating a first derivative of something labeled altitude in LidarLitePwm::getAcceleration() (which would be velocity, and not acceleration) and later integrating it to get something labeled velocity and (which would in fact be altitude).

So either your variable and method names are wrong, or you are missing one more derivative in LidarLitePwm::getAcceleration(). Also it is a litle weird that you are using the time when integrating acceleration and not when differentiating it. It should be the same in both directions. If you include the time in seconds, your units will be cm/s/s as stated in the comments, if you don't they will be cm/tick/tick.

Since it seems that you want to reach velocity as the final product anyway, I would skip calculating the accelerations, rename LidarLitePwm::getAcceleration() to LidarLitePwm::getVelocity() and change it to something like this:

float LidarLitePwm::getVelocity(float time)
    int currentAltitude = read();
    float velocity = (currentAltitude - _oldAltitude) / time;
    _oldAltitude = currentAltitude;

    return velocity; //cm/s/s

... all this assuming that the read() function returns distance from ground in cm.

The accelerometer stuff seems ok.

Btw if you are holding altitude with PID don't you need the altitude rather than velocity?

  • $\begingroup$ Thanks, you are right about the lidar stuff, I have updated the question to reflect this. Im holding altitude using 2 PID's. Once for altitude and one for rate of vertical change. I have got the altitude already from the lidar sensor and also from a barometer and GPS. $\endgroup$ Mar 26, 2015 at 8:27

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.