Calculating acceleration and velocity

Question

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];
    _freeImu.getQ(q);

    //Get raw data
    float values[9];
    _freeImu.getValues(values);

    //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

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?