Gyroscope Kalman Filter

Hello,

I’d like to be able to tell the exact angle that the robot is facing at a given time. So even if the robot is moving and rotating at the same time, I should be able to tell which way the robot is facing.

We purchased a gyroscope but the error accumulates very quickly in practice and we’re left with unusable values.

How would I go about making the values more accurate? I’ve read about Kalman filters which would use an accelerometer to correct the error. Would this work in practice with a Vex accelerometer and could anyone help me out with the (RobotC) programming for this? I’m a decent programmer but the kalman filter is a bit over my head.

Thanks,
Dan

I use the following code for the gyro, it still needs some refinement as the original was written several months ago but I find it adequate for a 3 minute match. Not sure that a kalman filter is really necessary, make sure the gyro has a solid mount, use a short cable to the cortex and keep sources of interference away from it (ie. don’t run near motor wires). The robot should be stationary when the gyro is initialized. This code probably needs ROBOTC 3.50 or later.

Init the gyro as follows, GyroInput is the analog sensor port.

    // Gyro init
    GyroInit( GyroInput );

Call GyroGetAngle() to get the current gyro value in degrees. Here was some code where I used it in some field centric drive code using mecanum wheels.

void
DriveSystemMecanumTask()
{
    int forward, right, turn;
    int temp;
    float theta;

    // Get joystick values
    DriveSystemGetControlValues( &forward, &right, &turn );

    // Field centric control
    if( theDrive.type == Mecanum4MotorFieldCentric )
    {
        // Get gyro angle in radians
        theta = degreesToRadians( GyroGetAngle() );

        // rotate coordinate system
        temp  = forward * cos(theta) - right * sin(theta);
        right = forward * sin(theta) + right * cos(theta);
        forward = temp;
    }

    // Send to drive
    DriveSystemMecanumDrive( forward, turn, right );
}

Here’s the library.

/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*                                                                             */
/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*                        Copyright (c) James Pearman                          */
/*                                   2012                                      */
/*                            All Rights Reserved                              */
/*                                                                             */
/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*        Module:     GyroLib.c                                                */
/*        Author:     James Pearman                                            */
/*        Created:    2 Oct 2012                                               */
/*                                                                             */
/*        Revisions:  V0.1                                                     */
/*                                                                             */
/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*        Description:                                                         */
/*                                                                             */
/*        Make using the Gyro easier                                           */
/*                                                                             */
/*-----------------------------------------------------------------------------*/
/*                                                                             */

// Stop recursive includes
#ifndef __GYROLIB__
#define __GYROLIB__

// Structure to hold global info for the gyro
typedef struct {
    tSensors port;
    bool     valid;
    float    angle;
    float    abs_angle;
    } gyroData;

static  gyroData    theGyro = {in1, false, 0.0, 0.0};

/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*  Small debugging function                                                   */
/*                                                                             */
/*-----------------------------------------------------------------------------*/

void
GyroDebug( int displayLine )
{
    string str;

    if( theGyro.valid )
        {
        // display current value
        sprintf(str,"Gyro %5.1f   ", theGyro.angle );
        displayLCDString(displayLine, 0, str);
        }
    else
        displayLCDString(displayLine, 0, "Init Gyro.." );
}

/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*  Task that polls the Gyro and calculates the angle of rotation              */
/*                                                                             */
/*-----------------------------------------------------------------------------*/

task GyroTask()
{
    int     gyro_value;
    int     gyro_error = 0;
    int     lastDriftGyro = 0;

    float   angle;
    float   old_angle, delta_angle;

    long    nSysTimeOffset;

    // Gyro readings invalid
    theGyro.valid = false;

    // Cause the gyro to reinitialize (a theory anyway)
    SensorType[theGyro.port] = sensorNone;

    // Wait 1/2 sec
    wait10Msec(50);

    // Gyro should be motionless here
    SensorType[theGyro.port] = sensorGyro;

    // Wait 1/2 sec
    wait10Msec(50);

    // What is the current system timer
    nSysTimeOffset = nSysTime;

    // loop forever
    while(true)
        {
        // get current gyro value (deg * 10)
        gyro_value = SensorValue[theGyro.port];

        // Filter drift when not moving
        if( (nSysTime - nSysTimeOffset) > 250 )
            {
            if( abs( gyro_value - lastDriftGyro ) < 3 )
                gyro_error += (lastDriftGyro - gyro_value);

            lastDriftGyro = gyro_value;

            nSysTimeOffset = nSysTime;
            }

        // Create float angle, remove offset
        angle = (gyro_value + gyro_error)  / 10.0;

        // normalize into the range 0 - 360
        if( angle < 0 )
            angle += 360;

        // store in struct for others
        theGyro.angle = angle;

        // work out change from last time
        delta_angle = angle - old_angle;
        old_angle   = angle;

        // fix rollover
        if(delta_angle > 180)
          delta_angle -= 360;
        if(delta_angle < -180)
          delta_angle += 360;

        // store absolute angle
        theGyro.abs_angle = theGyro.abs_angle + delta_angle;

        // We can use the angle
        theGyro.valid = true;

        // Delay
        wait1Msec( 20 );
        }
}

/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*      Initialize the Gyro on the given port                                  */
/*                                                                             */
/*-----------------------------------------------------------------------------*/

void
GyroInit( tSensors port  )
{
    theGyro.port = port;

    StartTask( GyroTask );
}

/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*  Cause the gyro to be reinitialized by stopping and then restarting the     */
/*  polling task                                                               */
/*                                                                             */
/*-----------------------------------------------------------------------------*/

void
GyroReinit()
{
    StopTask( GyroTask );
    StartTask( GyroTask );
}

/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*  Functions to get the public parameters                                     */
/*                                                                             */
/*-----------------------------------------------------------------------------*/

float
GyroGetAngle()
{
    return( theGyro.angle );
}
float
GyroGetAbsAngle()
{
    return( theGyro.abs_angle );
}

bool
GyroGetValid()
{
    return( theGyro.valid );
}


#endif  //__GYROLIB__


GyroLib.c (6.93 KB)

@jpearman:
Just wanted to make sure I’m reading your code correctly.

The idea you’re working with is that the gyro’s drift is slow. Thus, if the change in gyro readings is less than 3 of some unit (degrees, I think? Or is that 10ths of a degree?) every quarter-second, ignore that change, as it must be drift.

I’m kinda curious to see how others are compensating for drift. I had written something similar to your code when we got our first gyro, but was wondering if there was a different way…

EDIT:
@drdanielfc:
jpearman’s code should do what you’re asking, but if you want to read more about the gyro’s drift, I dug up a couple threads from the past:
https://vexforum.com/t/gyroscope-issues/22220/1
https://vexforum.com/t/gyro-sensor-problem/20214/1
[https://vexforum.com/t/gyro-sensor-drift/20155/1

//Andrew](https://vexforum.com/t/gyro-sensor-drift/20155/1)

https://vexforum.com/t/gyro-sensor-problem/20214/1
[https://vexforum.com/t/gyro-sensor-drift/20155/1

//Andrew](https://vexforum.com/t/gyro-sensor-drift/20155/1)

Are others still experiencing drift with the newer versions of ROBOTC? I was working with our Gyroscopic Sensor earlier with v3.54, and I had absolutely no drift.

~Jordan

I honestly don’t know at the moment, I bounce around between three or four different versions of ROBOTC depending on whether I’m writing code for myself, answering questions or beta testing new versions, it’s really hard to keep track of everything. The code I posted was probably written for V3.04 last year and then revised when V3.51 was released, I do seem to remember removing the drift code at one point but decided it still worked better with it left in. I will take a look later but I think that the last time I used it drift for steady state (no movement) was pretty good but I was still accumulating an error after driving the robot for a while. The team had a joystick button that would reinitialize the gyro if the error had become large during driving but then decided that they did not want to use it during the driver control period anyway so it was turned off.

Edit:

Ok, so I let the gyro sit for 10 minutes with no movement using V3.51 and there was absolutely no drift. The drift monitoring code won’t hurt anything, it will just leave the detected error at zero so there’s no downside to leaving it in other than very slow movement (0.8 deg/sec) would be ignored.

Why not use an electronic compass instead? It reports the angles between 0 and 360 that the robot is currently facing relative from Magnetic North.

I used this compass in a (non VRC) robot competition, recently. This worked fairly nicely, but I found it was only accurate +/- 2 to 3 degrees. (It would flutter back and forth a bit.) This wasn’t really a problem for me, but could be depending on how much you need a stable reading. If you do decide to go the compass route, make sure you get one with the accuracy you want. :slight_smile:

Of course, if you’re in VRC (non-college) you can’t use a compass anyway.

//Andrew

This is a device I’m currently using in a non-robotics application (digital camera metadata)
http://tech.yostengineering.com/3-space-sensor/product-family/embedded
Combo compass, gyro and accelerometer with on board processing. It barely works for my application and tends to be influenced by adjacent metal structure over which I have no control. Not legal for High School and would be expensive if sold as an accessory.

1 Like

I would suggest looking into Complementary filtering which can be just as effective/efficient as Kalman filtering yet MUCH less computationally intensive and MUCH less complicated…

Best