加速度・ジャイロによるカルマンフィルタ―用ライブラリ
Dependents: Kalman_test 201903_pf_lower_judgment_sashida 201903_pf_lower_judgment 201903_pf_lower_judgment
Kalman.cpp
- Committer:
- mikawataru
- Date:
- 2018-01-19
- Revision:
- 0:3e14df89021f
File content as of revision 0:3e14df89021f:
#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; };