SHARE
TWEET

Untitled

a guest May 19th, 2019 69 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include "DKalman.h"
  2.  
  3. DKalman::DKalman()
  4. {
  5.     x_next = 0.0;
  6.     p_next = 0.0;
  7.     K = 0.0;
  8.  
  9.     reset(0.0);
  10. }
  11.  
  12. DKalman::~DKalman()
  13. {
  14. }
  15.  
  16. double DKalman::cal(double val)
  17. {
  18.     x_next = x;
  19.     p_next = P + Q;
  20.     K = p_next / (p_next + R);
  21.     x = x_next + K *(val - x_next);
  22.     P = (1 - K) * p_next;
  23.  
  24.     return x;
  25. }
  26.  
  27. void DKalman::set_PQR(double p, double q, double r)
  28. {
  29.     P = p;
  30.     Q = q;
  31.     R = r;
  32. }
  33.  
  34. void DKalman::reset(double val)
  35. {
  36. //  P = 1.0;
  37. //  Q = 1.0 / 100000.0;
  38. //  R = 0.5;//0.5;
  39. //  x = val;
  40.  
  41.     P = 1.0;
  42.     Q = 1.0 / 1000.0;
  43.     R = 0.1;//0.5;
  44.     x = val;
  45. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top