Guide to gyro and accelerometer with Arduino including Kalman filtering

Tell me if I have configured it correctly

Acc_Gyro Arduino
3.3V <—> 3.3V
GND <—> GND
X <—> AN0
Y <—> AN1
Z <—> AN2
SDA <—> AN4
SCL <—> AN5

@psyoptica
Yes that looks correct. But beware that if there is no logic level circuit on your breakout board, you might damage the IC. So get a logic level converter if you need to use if on the long run: https://www.sparkfun.com/products/8745.

I don't think there is a need for any logic level converter device since the arduino I'm using already gives a 3.3V signal.

Thanks for the quick responses. Really hope I learn something from this tutorial.

@psyoptica
You are using an Arduino Uno, right?
Then there is need for a logic level converter! Since the ATmega328P is running at 5V.

Thanks for clearing it up. Will definitely have to get a converter circuit then..

I see a lot of experience embedded in this thread, so I have a question about any noticed issues with the LSM303DLM or DLH regarding bad readings in abrupt changes in orientation. Abrupt is defined as peak accelerations of about 3 g's in all directions. I am experiencing abrupt / large heading changes in a tilt compensated compass implemented with this device. In relatively "flat" conditions I don't see failures, but in severe conditions, especially sharp pitch (dx/dt) and sharp roll (dy/dt) I seem to get non linear readings. I say I seem, because as of yet I have not captured the case live as trials are expensive to duplicate in my moving platform.

My question is two fold:

  1. Has anyone experienced big non linear results in the accelerometers ?
  2. Has anyone noticed big non linear results in the magnetometer that are somehow induced with strong acceleration ?

My application is using a 9DOF IMU and I have wrapped the sensors in filters that limit rate of change of data as well as do a variety of selectable filter algorithms.

Regards,
Brian

Hi Lauszus,

I have read your codes about the gyro sensor and it's very helpfull. So thank you about it. Now i try to modify your code to my quadrotor project but it stills have some problem about "gyro drift". I know that i have to use the Kalmal filter but my board just only has 3 angular rate sensors (the gyro sensor as i think) and have no accelerometer, so i don't know how to use the Kamal filter. Could you take a look at my problem in here:
http://arduino.cc/forum/index.php/topic,156912.0.html

And could you show me some simple equation like the Complementary Filter which can aplly to my board.

Regards,
Nghia

@brianb00
I haven't got much experience with magnetometers, sorry.

@doantrungnghia05
The Kalman I filter I wrote can't be used without an accelerometer. The easiest thing would be to use a digital high-pass filter.

Lauszus:
@brianb00
I haven't got much experience with magnetometers, sorry.

@doantrungnghia05
The Kalman I filter I wrote can't be used without an accelerometer. The easiest thing would be to use a digital high-pass filter.

Thank Lauszus, here is my board, i bought it because in the internet many people use this board and they also buid their quad fly, but they just assembly these things like: motors, rf-controller, ...together without writing any code for the uController.

1.Kk_board datasheet: kkmulticopter.com

2.The angular rate sensor use in kk board: (ENC series) http://www.murata.com/products/catalog/pdf/s42e.pdf

My code is C++ for atmega168pa and i know this forum is about adruino, but i think my code is very cleary and easy to understand. Here is it:

/************************************************************************/
/*  MAIN FUNTION                                                       */
/************************************************************************/
int main(void)
{
	CLKPR=0x80;
	CLKPR=0x00;
	
	//----------- system init --------------------
	sei();
	usart_init();
	adc_sensor_init();
	led_init();
	motor_control_init();
	motor_start();
	DDRB|=(1<<PINB3);// measure loop time for cheking
	//-----------------------------------------------------------
	//---------- update the zero value ---------
	adc_sensor_get_value();
	roll_zero=roll_adc;//~=824
	yaw_zero=yaw_adc;//~=816
	pitch_zero=pitch_adc;//~=846
	roll_angle=360;
	yaw_angle=360;
	pitch_angle=360;
	//----------------------------------------------------------
	timer2_start();//start measure system time by timer2
	_delay_ms(2000);
	led_on();
	 while(1)
    {
		//--------------   get value in 3 sensors -----------------------
		PORTB^=(1<<PINB3);// toogle this pin per main loop
		adc_sensor_get_value();//
		
		//-------- just process roll gyro--------------------------------------------------------------
		if(roll_adc<roll_zero)
		{
			roll_rate=(roll_zero-roll_adc)/sensitivity;//tinh toc do goc theo phuong roll [degree/s]
			roll_angle-=roll_rate*dtime/1000;//tra ve gia tri degree theo phuong roll [degree]	
		}
		
		if(roll_adc>roll_zero)
		{
			roll_rate=(roll_adc-roll_zero)/sensitivity;//tinh toc do goc theo phuong roll [degree/s]
			roll_angle+=roll_rate*dtime/1000;//tra ve gia tri degree theo phuong roll	[degree]
		}
		//---------------------------------------------------------------------------------------------
		
		usart_clr();		
		usart_print_nb(roll_angle);//print roll_angle to computer screen
		
		//----------------------- creat fixed_loop_time= 25ms per loop---------------------------------	
		loop_usefull_time=sys_time-last_time;
		if(loop_usefull_time<fixed_loop_time)
		{
			for(i=0;i<(fixed_loop_time-loop_usefull_time);i++)
			{
				_delay_ms(1);
			}
		}		
		dtime=sys_time-last_time;//dtime=time per main loop (ms)
		last_time=sys_time;//update the last_time value for the next loop
		//------------------------------------------------------------------------------------------	
    }
}

ISR(TIMER2_OVF_vect)//when the timer2 over flow we have: sys_time=sys_time +1(ms)
{				// so we can caculate the dtime( time per main loop)
	sys_time+=1;  
	TCNT2=130;
}

In my board, the three gyro sensors also have the high-pass filter as you say, but it still drifs.
Could you take a look at my "gyro drift" problem and show me how to fix it ?

Regrard,
Nghia

@doantrungnghia05
I'm pretty sure the boards which only got a gyro on them don't even convert the gyro readings into degrees, but uses the rate as the input to there PID loop.

You can find plenty of firmware for you board. There is no need to write your own. For instance I found this using Google: GitHub - DaneGardner/KKMulticontroller: kkmulticopter flight controller software

You should ask at a quadrocopter forum or something like that instead.

Hi Lauszus
when i try to compile your sketch for the mpu6050 i get the error Kalman does not name a type and line 4 is flashing , i am new to Arduino and not shure how to correct it,

thanks Garwalt

@garwalt
Remember to download this file as well: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/Kalman.h

Thanks Lauszus i had thet file but it didnt work, so i downloaded it again and all is well.
I want to add servo controll to this sketch could you give advice on how i might do this?.

Thanks

@garwalt
Simply use the output from the Kalman filter as you input to your servo. See: Servo - Arduino Reference

Thanks Lauszus , sorry to be a pain but this is very new to me, the only thing i have a problem with is, is where in the sketch, is the best place to put the servo write command, at the moment i am using this line from another post.

servo1.writeMicroseconds(1560+kalAngleX*9.55);

i have tried it in various positions within the loop function, and all it does is send the servo full one way , i am getting data on the serial moniter.

Thanks

@garwalt
Simply add:

servo1.write(kalAngleX/2.0);
servo2.write(kalAngleY/2.0);

Just below:

timer = micros();

Thanks Lauszus, all is working as it should.

Hi Laszus the mpu6050 is working fine with your sketch, is there any way to match the angle of the servo to the angle of the gyro, at the moment if the gyro tilts to say 45 degrees the servo only moves to approx 30 degrees.

@garwalt
Just multiply the angle by a constant, as all Servos are a little different.
If you want more control use the writeMicroseconds function: Servo - Arduino Reference.

For instance you could fine the maximum and minimum values and then use map (map() - Arduino Reference) to map the angle to these values.

The post is very informative. Thank you so much.
I am working on something similar to it.

I am working on a self-balancing bicycle which is my final year project.The main idea is to run the bicycle,powered by a motor, in balanced position by measuring the tilt when ever the cycle tend to fall and using steering control to balance it. In my project, I am using two sensors gyro meter

and accelerometer
http://robokits.co.in/shop/index.php?main_page=product_info&cPath=11&products_id=39

to measure the tilt angle.

roll angle of the gyro along the front wheel and 'x-axis' of the accelerometer perpendicular to roll angle,towards the right side of the cycle,parallel to the ground so the roll angle will give the degree/sec and accelerometer will give the displaced angle or tilt angle when the bicycle tends to fall. 'y-axis' of the accelero also comes along the roll angle(I m not using it).

so, firstly I want to know whether the program you used to measure the tilt angle (in your case front and back) is applicable to sensors to accurately measure the tilt angle? (I m not using a imu). It would be of great help if you can make me figure out working of my sensors properly.

nowhere i could get the information about how you placed the axis of gyro and accelero....can you please tell me

please help me