38 double getAngle(
double newAngle,
double newRate,
double dt) {
46 rate = newRate - bias;
51 P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
52 P[0][1] -= dt * P[1][1];
53 P[1][0] -= dt * P[1][1];
54 P[1][1] += Q_bias * dt;
59 S = P[0][0] + R_measure;
73 P[0][0] -= K[0] * P[0][0];
74 P[0][1] -= K[0] * P[0][1];
75 P[1][0] -= K[1] * P[0][0];
76 P[1][1] -= K[1] * P[0][1];
80 void setAngle(
double newAngle) { angle = newAngle; };
84 void setQangle(
double newQ_angle) { Q_angle = newQ_angle; };
85 void setQbias(
double newQ_bias) { Q_bias = newQ_bias; };
86 void setRmeasure(
double newR_measure) { R_measure = newR_measure; };
double getRmeasure()
Definition: Kalman.h:90
void setRmeasure(double newR_measure)
Definition: Kalman.h:86
double getQangle()
Definition: Kalman.h:88
double getAngle(double newAngle, double newRate, double dt)
Definition: Kalman.h:38
void setAngle(double newAngle)
Definition: Kalman.h:80
double getRate()
Definition: Kalman.h:81
void setQangle(double newQ_angle)
Definition: Kalman.h:84
void setQbias(double newQ_bias)
Definition: Kalman.h:85
double getQbias()
Definition: Kalman.h:89
Kalman()
Definition: Kalman.h:23