After a few more tests, I now have the final version of the ArduIMU. The main problem was the calibration of the gyros: converting the raw data to degrees/s. I had to change the source code, compile it, upload, and repeat it over and over again. Totally waste of time.

So i configured Arduino to externally adjust the gyros and the accelerometer jitter filter, using the nunchuck potentiometers (joystick axis). Now it’s working pretty well. =)

Please, in the source code, ignore all the lines marked with "//(kalman)", this lines are related to the kalman filter code, those marked by "//(nunchuck) are related to nunchuck accelerometer and axis control, and all marked by "//(gyro)" are code related to the control of the gyroscope.

Another tip, all the data is processed in radians, then is converted to degrees (radian*180/PI = radians*57.295779= degrees). If somebody needs any help, please feel free of asking me.

Video:

-Yellow color = Just Accelerometer read
-Red color= Accelerometer+Gyro+KalmanFIiter

Arduino Source Code:
http://sites.google.com/site/jordiuavs/Home/kalman_filter_by_Jordi.txt



Labview (ground station) source code:
http://sites.google.com/site/jordiuavs/Home/kalman_filter_by_Jordi.txt

To do: -Integrate another Gyro (to make it dual axis) . -Program the SPI altimeter.
-And put all the hardware together and make it work (GPS, Altimeter, IMU, Modem).

Done: -Servo control -Gps Waypoint navigation (I’ll post later about it).

The next article I'm going to post all the links to get started with Arduino environment. All you would need to learn: tutorials, examples, references, tips and tricks. If you already know C++ it’s going to be 99% easier. If not, you are going to discover why C/C++ is the most compressive and famous language ever ;-) Then jumping to any AVR, ARM, PIC microcontroller C/C++ language environment, is going to be a lot easier, because they’re all based on the same principles.

Views: 25668

Tags: kalman

Comment by Michele on February 2, 2008 at 3:00am
wow!!, very, very compliments!!!
Comment by Jack Crossfire on February 8, 2008 at 5:23pm
Been 20 years since I saw "joy_x" in a piece of source code.
Comment by Christoph Prantl on March 31, 2008 at 5:37am
Hi Jordi,
I loke your work very much :-)
I'm pretty new in this UAV thingy and want to start with a project similar like your arducopter but make it a bit more "generic".

-> the part i'm trying to do currently is to try to use Kalman filters as "library" or external functions in the Arduino environment (i want to just pass values and get the corrected flattened values as response)
Do you have an earlier version of the Kalman filter code around somewhere? without the specific raedings and settings for your hardware.
Cheers Chris ...

3D Robotics
Comment by Jordi Muñoz on March 31, 2008 at 10:40pm
Hello Chris,

Thanks for the compliment...

Yes i been working in a fuction like this:

Kalman(Gyro, AccelXY, AccelZ);

And the return will be the estimated position in radians... Is better because i save code for the two axis that i need to know (pitch and roll).

I have this for now, is the function:

float kalman (float q_m, float angle_m)
{

/* Unbias our gyro */
const float q = q_m - q_bias; //(Kalman)
const float Pdot[2 * 2] = {
Q_angle - P[0][1] - P[1][0], /* 0,0 */ //(Kalman)
- P[1][1], /* 0,1 */
- P[1][1], /* 1,0 */
Q_gyro /* 1,1 */
};

/* Store our unbias gyro estimate */
float rate = q; //(Kalman)

/*
* Update our angle estimate
* angle += angle_dot * dt
* += (gyro - gyro_bias) * dt
* += q * dt
*/
angle += q * dt; //(Kalman)

/* Update the covariance matrix */
P[0][0] += Pdot[0] * dt; //(Kalman)
P[0][1] += Pdot[1] * dt; //(Kalman)
P[1][0] += Pdot[2] * dt; //(Kalman)
P[1][1] += Pdot[3] * dt; //(Kalman)

R_angle= (joy_y_axis+1)*0.0098039; //external adjust jitter of accelerometer


float angle_err = angle_m - angle; //(Kalman)
float C_0 = 1; //(Kalman)
float PCt_0 = C_0 * P[0][0]; //(Kalman)
float PCt_1 = C_0 * P[1][0]; //(Kalman)
float E =R_angle+ C_0 * PCt_0; //(Kalman)
float K_0 = PCt_0 / E; //(Kalman)
float K_1 = PCt_1 / E; //(Kalman)
float t_0 = PCt_0; /* C_0 * P[0][0] + C_1 * P[1][0] (Kalman) */

const float t_1 = C_0 * P[0][1]; /* + C_1 * P[1][1] = 0 (Kalman) */


P[0][0] -= K_0 * t_0; //(Kalman)
P[0][1] -= K_0 * t_1; //(Kalman)
P[1][0] -= K_1 * t_0; //(Kalman)
P[1][1] -= K_1 * t_1; //(Kalman)
angle += K_0 * angle_err; //(Kalman)
q_bias += K_1 * angle_err; //(Kalman)



return angle*RadToDeg;
}


Good Luck!

3D Robotics
Comment by Jordi Muñoz on March 31, 2008 at 10:43pm
Sorry my function is diferent you need to give the pitch or roll angle obtained just with the accelerometer:

angle_m= arcTan(axisX,AxisZ);

X= is for the pitch
Y=For the roll
Depends in the way you positioned your accel..
Comment by Christoph Prantl on April 1, 2008 at 2:49am
Hi Jordy,
Thank you so much ... I try to figure this out now.

Currently my only plan is to provide nick/roll stabilization (my 4 year old son wants to fly my heli :-))
and the gear is stabilized by a AVCS (Angular Velocity Control System) gyro anyway. And therefore i just want to use the Kalman filter for nick and roll "denoising".
Comment by Christoph Prantl on April 1, 2008 at 6:31am
I just set up a blog for the project:
Maybe you can take advantage of what I found out and did so far ... (of course with credits to all the people i use code of.)
I am going the "library" and function approach.

And the Heli i got is cheap and reliable for this :-)
Comment by Mitch on April 14, 2008 at 7:20pm
Thank you for your great work. I am going to use this as a base to make a Arduino IMU based on a Sparkfun 5DOF.
Comment by John Crisford on April 22, 2008 at 4:31pm
Will the bluetooth arduino be fast enough to get the data from sparkfun 6dof bluetooth and process through a kalman filter on 3 axis?
Comment by carmine on May 10, 2008 at 7:27am
hi.very good job. I think i need you. I'm doing a INS/GPS integrated positioning system with Kalman filtrer in C code for a little ship. Do you have somethingh for help me?
thank you very very much from Italy.

Comment

You need to be a member of DIY Drones to add comments!

Join DIY Drones

Groups

Season Two of the Trust Time Trial (T3) Contest 
A list of all T3 contests is here. The current round, the Drone Delivery Challenge, is here

© 2014   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service