Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include "DKalman.h"
- DKalman::DKalman()
- {
- x_next = 0.0;
- p_next = 0.0;
- K = 0.0;
- reset(0.0);
- }
- DKalman::~DKalman()
- {
- }
- double DKalman::cal(double val)
- {
- x_next = x;
- p_next = P + Q;
- K = p_next / (p_next + R);
- x = x_next + K *(val - x_next);
- P = (1 - K) * p_next;
- return x;
- }
- void DKalman::set_PQR(double p, double q, double r)
- {
- P = p;
- Q = q;
- R = r;
- }
- void DKalman::reset(double val)
- {
- // P = 1.0;
- // Q = 1.0 / 100000.0;
- // R = 0.5;//0.5;
- // x = val;
- P = 1.0;
- Q = 1.0 / 1000.0;
- R = 0.1;//0.5;
- x = val;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement