Wataru Mikawa
加速度・ジャイロによるカルマンフィルタ―用ライブラリ
Dependents: Kalman_test 201903_pf_lower_judgment_sashida 201903_pf_lower_judgment 201903_pf_lower_judgment
Revision 0:3e14df89021f, committed 2018-01-19
- Comitter:
- mikawataru
- Date:
- Fri Jan 19 10:41:41 2018 +0000
- Commit message:
- initialize
Changed in this revision
| Kalman.cpp | Show annotated file Show diff for this revision Revisions of this file |
| Kalman.h | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman.cpp Fri Jan 19 10:41:41 2018 +0000
@@ -0,0 +1,57 @@
+#include "Kalman.h"
+
+KalmanFilter::KalmanFilter() {
+ angle = 0.0;
+ bias = 0.0;
+ P[0][0] = 0.0;
+ P[0][1] = 0.0;
+ P[1][0] = 0.0;
+ P[1][1] = 0.0;
+};
+
+void KalmanFilter::setAngle(float newAngle) {
+ angle = newAngle;
+};
+
+float KalmanFilter::calcAngle(float newAngle, float newRate, float dt) {
+
+ // variances
+ float Q_angle = 0.001;
+ float Q_bias = 0.003;
+ float R_measure = 0.03;
+
+ // step 1
+ float rate = newRate - bias;
+ angle += dt * rate;
+
+ // step 2
+ P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle);
+ P[0][1] -= dt * P[1][1];
+ P[1][0] -= dt * P[1][1];
+ P[1][1] += Q_bias * dt;
+
+ // step 3
+ float y = newAngle - angle;
+
+ // step 4
+ float S = P[0][0] + R_measure;
+
+ // step 5
+ float K[2];
+ K[0] = P[0][0] / S;
+ K[1] = P[1][0] / S;
+
+ // step 6
+ angle += K[0] * y;
+ bias += K[1] * y;
+
+ // step 7
+ float P00_temp = P[0][0];
+ float P01_temp = P[0][1];
+ P[0][0] -= K[0] * P00_temp;
+ P[0][1] -= K[0] * P01_temp;
+ P[1][0] -= K[1] * P00_temp;
+ P[1][1] -= K[1] * P01_temp;
+
+ return angle;
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman.h Fri Jan 19 10:41:41 2018 +0000
@@ -0,0 +1,15 @@
+#ifndef _KALMAN_H
+#define _KALMAN_H
+
+class KalmanFilter {
+private:
+ float angle;
+ float bias;
+ float P[2][2];
+public:
+ KalmanFilter();
+ void setAngle(float newAngle);
+ float calcAngle(float newAngle, float newRate, float dt);
+};
+
+#endif
\ No newline at end of file