Wataru Mikawa / Kalman

加速度・ジャイロによるカルマンフィルタ―用ライブラリ

Dependents:   Kalman_test 201903_pf_lower_judgment_sashida 201903_pf_lower_judgment 201903_pf_lower_judgment

Kalman.cpp@0:3e14df89021f, 2018-01-19 (annotated)

Committer:
mikawataru
Date:
Fri Jan 19 10:41:41 2018 +0000
Revision:
0:3e14df89021f
initialize

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mikawataru 0:3e14df89021f 1 #include "Kalman.h"
mikawataru 0:3e14df89021f 2
mikawataru 0:3e14df89021f 3 KalmanFilter::KalmanFilter() {
mikawataru 0:3e14df89021f 4 angle = 0.0;
mikawataru 0:3e14df89021f 5 bias = 0.0;
mikawataru 0:3e14df89021f 6 P[0][0] = 0.0;
mikawataru 0:3e14df89021f 7 P[0][1] = 0.0;
mikawataru 0:3e14df89021f 8 P[1][0] = 0.0;
mikawataru 0:3e14df89021f 9 P[1][1] = 0.0;
mikawataru 0:3e14df89021f 10 };
mikawataru 0:3e14df89021f 11
mikawataru 0:3e14df89021f 12 void KalmanFilter::setAngle(float newAngle) {
mikawataru 0:3e14df89021f 13 angle = newAngle;
mikawataru 0:3e14df89021f 14 };
mikawataru 0:3e14df89021f 15
mikawataru 0:3e14df89021f 16 float KalmanFilter::calcAngle(float newAngle, float newRate, float dt) {
mikawataru 0:3e14df89021f 17
mikawataru 0:3e14df89021f 18 // variances
mikawataru 0:3e14df89021f 19 float Q_angle = 0.001;
mikawataru 0:3e14df89021f 20 float Q_bias = 0.003;
mikawataru 0:3e14df89021f 21 float R_measure = 0.03;
mikawataru 0:3e14df89021f 22
mikawataru 0:3e14df89021f 23 // step 1
mikawataru 0:3e14df89021f 24 float rate = newRate - bias;
mikawataru 0:3e14df89021f 25 angle += dt * rate;
mikawataru 0:3e14df89021f 26
mikawataru 0:3e14df89021f 27 // step 2
mikawataru 0:3e14df89021f 28 P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle);
mikawataru 0:3e14df89021f 29 P[0][1] -= dt * P[1][1];
mikawataru 0:3e14df89021f 30 P[1][0] -= dt * P[1][1];
mikawataru 0:3e14df89021f 31 P[1][1] += Q_bias * dt;
mikawataru 0:3e14df89021f 32
mikawataru 0:3e14df89021f 33 // step 3
mikawataru 0:3e14df89021f 34 float y = newAngle - angle;
mikawataru 0:3e14df89021f 35
mikawataru 0:3e14df89021f 36 // step 4
mikawataru 0:3e14df89021f 37 float S = P[0][0] + R_measure;
mikawataru 0:3e14df89021f 38
mikawataru 0:3e14df89021f 39 // step 5
mikawataru 0:3e14df89021f 40 float K[2];
mikawataru 0:3e14df89021f 41 K[0] = P[0][0] / S;
mikawataru 0:3e14df89021f 42 K[1] = P[1][0] / S;
mikawataru 0:3e14df89021f 43
mikawataru 0:3e14df89021f 44 // step 6
mikawataru 0:3e14df89021f 45 angle += K[0] * y;
mikawataru 0:3e14df89021f 46 bias += K[1] * y;
mikawataru 0:3e14df89021f 47
mikawataru 0:3e14df89021f 48 // step 7
mikawataru 0:3e14df89021f 49 float P00_temp = P[0][0];
mikawataru 0:3e14df89021f 50 float P01_temp = P[0][1];
mikawataru 0:3e14df89021f 51 P[0][0] -= K[0] * P00_temp;
mikawataru 0:3e14df89021f 52 P[0][1] -= K[0] * P01_temp;
mikawataru 0:3e14df89021f 53 P[1][0] -= K[1] * P00_temp;
mikawataru 0:3e14df89021f 54 P[1][1] -= K[1] * P01_temp;
mikawataru 0:3e14df89021f 55
mikawataru 0:3e14df89021f 56 return angle;
mikawataru 0:3e14df89021f 57 };