Diff: KalmaFilterPulse.cpp

Revision:

1:f696623db4b5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/KalmaFilterPulse.cpp	Tue Jun 05 04:17:31 2018 +0000
@@ -0,0 +1,36 @@
+/* 
+ * File:   KalmanFilterPulse.cpp
+ * Author: Duy Lion Tran
+ * 
+ * Created on July 12, 2016, 1:04 PM
+ */
+
+#include "KalmanFilterPulse.h"
+
+KalmanFilterPulse::KalmanFilterPulse(double q, double r, double p)
+    : _q(q), _q_init(q), _r(r), _r_init(0), _x(0), _p(p), _p_init(p), _k(_p / (_p + _r))
+{
+
+}
+
+KalmanFilterPulse::~KalmanFilterPulse() {
+}
+
+/**
+ * update data
+ * 
+ * @param measurement
+ * @return 
+ */
+double KalmanFilterPulse::kalmanUpdate(double measurement) {
+    //prediction update
+    //omit _x = _x
+    _p = _p + _q;
+
+    //measurement update
+    _k = _p / (_p + _r);
+    _x = _x + _k * (measurement - _x);
+    _p = (1 - _k) * _p;
+
+    return _x;
+}