succes
Embed:
(wiki syntax)
Show/hide line numbers
kalman.cpp
00001 #include "kalman.h" 00002 00003 // Kalman filter module 00004 // Shamelessly ripped from http://forum.arduino.cc/index.php/topic,8652.0.html 00005 00006 float Q_angle = 0.001; 00007 float Q_gyro = 0.003; 00008 float R_angle = 0.03; 00009 00010 float x_angle = 0; 00011 float x_bias = 0; 00012 float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0; 00013 float y, S; 00014 float K_0, K_1; 00015 00016 00017 float kalmanCalculate(float newAngle, float newRate, float dt) 00018 { 00019 // dt = float(looptime)/1000; 00020 x_angle += dt * (newRate - x_bias); 00021 P_00 += - dt * (P_10 + P_01) + Q_angle * dt; 00022 P_01 += - dt * P_11; 00023 P_10 += - dt * P_11; 00024 P_11 += + Q_gyro * dt; 00025 00026 y = newAngle - x_angle; 00027 S = P_00 + R_angle; 00028 K_0 = P_00 / S; 00029 K_1 = P_10 / S; 00030 00031 x_angle += K_0 * y; 00032 x_bias += K_1 * y; 00033 P_00 -= K_0 * P_00; 00034 P_01 -= K_0 * P_01; 00035 P_10 -= K_1 * P_00; 00036 P_11 -= K_1 * P_01; 00037 00038 return x_angle; 00039 }
Generated on Wed Jul 13 2022 09:29:34 by 1.7.2