#include <Kalman.h>
double Kalman::getAngle |
( |
double |
newAngle, |
|
|
double |
newRate, |
|
|
double |
dt |
|
) |
| |
|
inline |
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];
double Kalman::getQangle |
( |
| ) |
|
|
inline |
double Kalman::getQbias |
( |
| ) |
|
|
inline |
double Kalman::getRate |
( |
| ) |
|
|
inline |
double Kalman::getRmeasure |
( |
| ) |
|
|
inline |
90 {
return R_measure; };
void Kalman::setAngle |
( |
double |
newAngle | ) |
|
|
inline |
80 { angle = newAngle; };
void Kalman::setQangle |
( |
double |
newQ_angle | ) |
|
|
inline |
84 { Q_angle = newQ_angle; };
void Kalman::setQbias |
( |
double |
newQ_bias | ) |
|
|
inline |
85 { Q_bias = newQ_bias; };
void Kalman::setRmeasure |
( |
double |
newR_measure | ) |
|
|
inline |
86 { R_measure = newR_measure; };
The documentation for this class was generated from the following file:
- /Users/michaelbrookes/ClionProjects/Quadro/lib/external/Kalman.h