Accelerometers

Basic Model
The most basic representation of an accelerometer is the one showing a mass suspended with a spring system. The mass location along a single axis (named the sensitive axis) represents the measured acceleration. Notice that the mass will only change its location if the complete device is being accelerated (i.e. when a vehicle starts moving). However, once the acceleration effect is gone (i.e. vehicle is moving at constant speed or is stationary) the mass will return to its original location.

accel

Position Estimation?
In theory, the acceleration measurements could be added up, and the result will represent the velocity, and this velocity could also be added to obtain position. In the practice there are many things that affect the measurement, making this scheme quite difficult. When using low cost sensors, any small unaccounted acceleration error will create a velocity offset (hence a positioning error) that will be very difficult to detect unless other sensor modalities are available. There are also some rare conditions there are some techniques that make this process doable using Zero-velocity UPdaTe (ZUPT) techniques.

Tilt Estimation
It is easy to understand that if our accelerometer model with the mass and spring system is tilted, the gravity effect will make the mass move, and more importantly, the mass will remain in a position different that its origin, unless the complete system is tilted back. This happens, because gravity is a constant acceleration and by tilting the accelerometer, the sensitive axis will start getting some part of it.

The tilt angle is related to the accelerometer measurement using the following equation:
TILT_ANGLE = ARC_SIN( ACCELEROMETER_MEASUREMENT / GRAVITY );

Notice that in order to use this equation, the accelerometer measurement should only be measuring gravity, if a robot is moving at varying speeds while trying to measure tilt, this measurement will not be accurate.

Euler Angles
Notice that when I wrote the equation that measures tilt, and did not want to give it an specific name. However, when talking about tilt in the literature, they refer to the roll and pitch angles or Euler angles. The full interpretation of the Euler angles is somewhat involved, but with some assumptions their visualization is easier to understand.

PITCH = ARC_SIN( FORWARD_ACCELERATION / GRAVITY)

The roll angle, which corresponds to the side-to-side robot angle, is registered by the accelerometer with sensitive axis pointing towards the sides. The equation is slightly different:

ROLL = ARC_SIN ( LATERAL_ACCELERATION / ( GRAVITY * COS(THETA) ) )

Problems with Accelerometer measuring Tilt and Euler Representations
There are a couple of pressing issues with these equations. The first one occurs with the pitch angle is near the vertical. Because of the accelerometer noise, there is a good chance that the forward acceleration will register a a value grater than gravity. If that is the case the mathematical result of the arc-sin function is an imaginary number. In order to understand the second issue, lets suppose for a moment that our sensor is perfect (no noise). In that case, if the device is place parallel to the gravity, the pitch angle will report correctly a 90 degree value. However, since the cosine of 90 degrees equals zero, we will find that the roll angle is undefined. The Euler angles are not the only way to represent a robot orientation and tilt, however it is the easier to understand. Some readers interested in reading further, may need to learn about Quaternions or Cosine Direction Matrices (CDM). Luckily, in most robotic applications will not require having the robot pitch near the vertical, so we can use the simple equations shown above.

What About the Yaw Angle?
The yaw angle cannot be measured using accelerometers. For applications where a robot will be mostly used on an flat terrain, a single axis gyroscope with its sensitive axis perpendicular to the terrain can be used to measure this angle as it has been demonstrated in the Robot Navigation Software. The single gyroscope solution is generally acceptable for a robot that moves on a slightingly uneven terrain, and is also a valid solution in cases with significant tilt as long as the robot makes the turn on the same plane. For other more complex applications, the only viable solution is using a full Inertial Measurement Unit (IMU), which for now is out of the scope of this tutorials.

Testing Program
I created a program that computes the roll and pitch angle using accelerometer measurements. The program uses the Microinfinity XG1300L device, which include accelerometers and a single axis gyro.
All the necessary files can be found in the RobotNav Github repository. Keep in mind that using the LCD requires more than a single file to be compiled

The following is the main program file:

/*
 * Robot Navigation Program
 * www.robotnav.com
 *
 * (C) Copyright 2013 - 2014 Navigation Solutions, LLC
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <fcntl.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
#include <sys/mman.h>
#include <sys/ioctl.h>

#include "lms2012.h"
#include "d_lcd.h"

//Runtime constants
const float GRAVITY = 9.80297286843;
const float PI = 3.14159265358979;
const int MAX_SAMPLES = 100;

//Global variables and constants used by the sensor handling functions
const int XGL_PACKET_SIZE = 10; //2(gyro angle) + 2(gyro rate) + 2(acc x) + 2(acc y) + 2(acc z) 
const char XGL_PORT = 0x0; //EV3 label minus 1

int xgl_device_file;
IIC   *pXgl;

//INITIALIZE DEVICE
int init_xg1300l_gyro()
{
	IICDAT IicDat;
	//Open the device xgl_device_file
	if((xgl_device_file = open(IIC_DEVICE_NAME, O_RDWR | O_SYNC)) == -1)
	{
		printf("Failed to open device\n");
		return 0; 
	}
	pXgl = (IIC *)mmap(0, sizeof(IIC), PROT_READ | PROT_WRITE, MAP_FILE | MAP_SHARED, xgl_device_file, 0);
	if (pXgl == MAP_FAILED)
	{
		printf("Map failed\n");
		return 0;
	}
	//Setup IIC to read 2 packets
	//This part is only needed for devices that use more than one byte
	//or special configuration
	IicDat.Port = XGL_PORT;
	IicDat.Time = 0;
	IicDat.Repeat = 0;
	IicDat.RdLng = XGL_PACKET_SIZE;
	IicDat.WrLng = 2;
	// Set the device I2C address
	IicDat.WrData[0] = 0x01;
	// Specify the register that will be read (0x42 = angle)
	IicDat.WrData[1] = 0x42;
	// Setup I2C comunication
	ioctl(xgl_device_file,IIC_SETUP,&IicDat);
	printf("Device is ready\n");
	return 1;
}

//PROCESS SENSOR DATA
void get_xg1300l_gyro(float *angle, float *rate, float *acc_x, float *acc_y, float *acc_z)
{
	//Compute angle, angular rate and accelerations
	 *acc_z = (pXgl->Raw[XGL_PORT][pXgl->Actual[XGL_PORT]][0] * 256 + pXgl->Raw[XGL_PORT][pXgl->Actual[XGL_PORT]][1]) / 100.0;
	 *acc_y = (pXgl->Raw[XGL_PORT][pXgl->Actual[XGL_PORT]][2] * 256 + pXgl->Raw[XGL_PORT][pXgl->Actual[XGL_PORT]][3]) / 100.0;
	 *acc_x = (pXgl->Raw[XGL_PORT][pXgl->Actual[XGL_PORT]][4] * 256 + pXgl->Raw[XGL_PORT][pXgl->Actual[XGL_PORT]][5]) / 100.0;
	 *rate  = (pXgl->Raw[XGL_PORT][pXgl->Actual[XGL_PORT]][6] * 256 + pXgl->Raw[XGL_PORT][pXgl->Actual[XGL_PORT]][7]) / 100.0;
	 *angle = (pXgl->Raw[XGL_PORT][pXgl->Actual[XGL_PORT]][8] * 256 + pXgl->Raw[XGL_PORT][pXgl->Actual[XGL_PORT]][9]) / 100.0;
}

//CLOSE DEVICE
void close_xg1300l_gyro()
{
	printf("Clossing device\n");
	close(xgl_device_file);
}

int main()
{
	int i;
	float angle;
	float rate;
	float acc_x;
	float acc_y;
	float acc_z;
	char aux_buffer[200];
	float pitch;
	float roll;
	
	//Create a structure that will store the LCD information
	LCD my_lcd;
	//Initialize and clear screen
	dLcdInit(my_lcd.Lcd);
	LCDClear(my_lcd.Lcd);

	if(!init_xg1300l_gyro())
		return -1;

	for(i = 0;i<MAX_SAMPLES;i++)
	{
		get_xg1300l_gyro(&angle, &rate, &acc_x, &acc_y, &acc_z);
		if(fabs(acc_y) < GRAVITY) // Acceleration measurement cannot be larger than GRAVITY
		{ //Compute the PITCH angle
			pitch = asin(acc_y / GRAVITY) ;
			sprintf(aux_buffer,"Pitch: %0.2f [deg]   ", pitch * 180.0 / PI);
			dLcdDrawText(my_lcd.Lcd, FG_COLOR, 1, 20, NORMAL_FONT, (signed char *)aux_buffer);
			printf("%s\n", aux_buffer);
			if(fabs(acc_x) < GRAVITY)
			{ //Compute the ROLL angle
				roll = asin(acc_x / GRAVITY / cos(pitch));
				sprintf(aux_buffer,"Roll: %0.2f [deg]   ", roll * 180.0 / PI);
				dLcdDrawText(my_lcd.Lcd, FG_COLOR, 1, 40, NORMAL_FONT, (signed char *)aux_buffer);
				printf("%s\n", aux_buffer);
			}
			else
			{ //ROLL angle is undefined
				sprintf(aux_buffer,"Roll angle is undefined");
				dLcdDrawText(my_lcd.Lcd, FG_COLOR, 1, 40, NORMAL_FONT, (signed char *)aux_buffer);
				printf("%s\n", aux_buffer);
			}
		}
		else
		{ // YAW angle is approximately the integrated output of the Z-axis gyro,
			// this is NOT correct when there is significant roll or pitch angle changes while rotating the device
			sprintf(aux_buffer,"Roll and Pitch angle are undefined");
			dLcdDrawText(my_lcd.Lcd, FG_COLOR, 1, 20, NORMAL_FONT, (signed char *)aux_buffer);
			dLcdDrawText(my_lcd.Lcd, FG_COLOR, 1, 40, NORMAL_FONT, (signed char *)aux_buffer);
			printf("%s\n", aux_buffer);
		}
		sprintf(aux_buffer,"Yaw: %0.2f [deg]   ", angle);
		dLcdDrawText(my_lcd.Lcd, FG_COLOR, 1, 60, NORMAL_FONT, (signed char *)aux_buffer);
		printf("%s\n", aux_buffer);
		dLcdUpdate(&my_lcd);
		sleep(1);
	}

	//Close devices
	close_xg1300l_gyro();
	dLcdExit(); 
	return 1;
}