Advertisement
Guest User

Untitled

a guest
May 19th, 2019
95
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 0.52 KB | None | 0 0
  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. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement