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
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
Replies to This Discussion
-
Permalink Reply by wayne garris on -
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 -
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?
-
Permalink Reply by Krzysztof Bosak on -
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
-
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.
-
Permalink Reply by Krzysztof Bosak on
-
The method is to handle each euler angle separately, each kalman with 3 dofs.
-
Permalink Reply by Recmaster on -
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.
-
Permalink Reply by Krzysztof Bosak on
-
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.
-
Permalink Reply by wayne garris on
-
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 -
What are the dynamics and measurement equations you are using to feed this filter?
- ‹ Previous
- 1
- 2
- Next ›
Social Networking
Top Content
Contests
Season Two of the Trust Time Trial (T3) Contest has now begun. The fourth round is an accuracy round for multicopters, which requires contestants to fly a cube. The deadline is April 14th.A list of all T3 contests is here.
Groups
-
Documentation Editors
51 members
-
Moderators
24 members
-
ArduCopter User Group
1297 members
-
Australia
79 members
-
Andropilot Users Group
183 members
Advertisement
© 2013 Created by Chris Anderson.
Powered by
