Moderator

kalman filter for idiots

x_angle += dt * (gyro_angle - x_bias);
P_00 += - dt * (P_10 + P_01) + Q_angle * dt;
P_01 += - dt * P_11;
P_10 += - dt * P_11;
P_11 += + Q_gyro * dt;

y = acc_angle - x_angle;
S =P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;

x_angle += K_0 * y;
x_bias += K_1 * y;

P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;

x_angle = kalman filter state estimate
now was that realy so hard?
gyro_angle = gyro input in rad per sec
acc_angle = accel input in rad
dt=time since last loop
Q_angle= 0.01
Q_gyro= 0.03
R_angle = 0.7
x_angle = final estimate

Views: 558

▶ Reply to This

Replies to This Discussion


ModeratorPermalink Reply by wayne garris on December 23, 2008 at 6:24pm
i disagree as the gyro bias drifts way to much especially after a power cycle temp change ect. ya you might get away with using the same K and P after the converge but i promise you they don't converge the same every time i boot the system as far as worrying about processor speed that might be a problem if you want to use a small old 8 bit cpu but that whole paradigm is being relegated to the past.
Permalink Reply by dp on March 27, 2009 at 7:33am
I have writen the Kalman filter codes in C on my MCU LPC2114 ,but it seems that my MCU is not strong enough to run the codes. My 12 states SINS/GPS integration system can not work correctly, and the time the filter costs is too long.
Has any body ever did the same work?

ModeratorPermalink Reply by Krzysztof Bosak on March 27, 2009 at 1:09pm
12 states is 12x12 matrices, AFAIK. may or may not be too much for ARM7. What update rate (Hz) you want to achieve?
Try STR911 from ARM9, they have hardware floating point.
Permalink Reply by dp on March 28, 2009 at 5:39am
The update rate should be at least 20Hz.
And I want to know is there any method to improve the caculate rate?Except the hardware floating point unit.
I will try to port my programs to ARM9 later.Thanks.

ModeratorPermalink Reply by Krzysztof Bosak on March 28, 2009 at 9:53am
The method is to handle each euler angle separately, each kalman with 3 dofs.
Permalink Reply by Recmaster on October 27, 2009 at 4:17am
Hello Krzysztof,
is it difficult to add a floating-point co-processor into Arduino board and recompile existing code?
I mean uM-FPU v3.1 for example, that exist on Sparkfun storage.

ModeratorPermalink Reply by Krzysztof Bosak on October 27, 2009 at 9:22am
For me it is trivial if I do it full time, but the protocols are difficult in the sense they are easy to be messed up. Somebody must do it very seriously. Any additional digital component is less forbidding than ADC input which at worst would get messy and noisy.

ModeratorPermalink Reply by wayne garris on October 27, 2009 at 9:30am
You can see it implemented with centripetal comp on 3 axis http://code.google.com/p/uax/
I use a netburner MOD5213 .
Permalink Reply by xchip on November 7, 2009 at 6:53am
What are the dynamics and measurement equations you are using to feed this filter?

RSS

Contests

Season Two of the Trust Time Trial (T3) Contest has now begun. The third round was a reliablilty/aerial photography round for both planes and copters, which is now closed. Stay tuned for the next round, beginning soon.

A list of all T3 contests is here. 

 

Top Content 

1 What do you think the APM should do on fail safe

What do you think the APM should do on fail safe
2 Hexa-copter found after failed attempt of delivering cell phones to inmates of a Brazilian penitentiary

Hexa-copter found after failed attempt of delivering cell phones to inmates of a Brazilian penitentiary
3 Gauging Interest in the CNC Machine

Gauging Interest in the CNC Machine
4 Housekeeping note: Kevin Finisterre

Housekeeping note: Kevin Finisterre
5 hexa copter calibration problem

hexa copter calibration problem

Groups

© 2012   Created by Chris Anderson.

Badges  |  Report an Issue  |  Terms of Service