Add matrix and kf code

include/sta/math/algorithms/kalmanFilter.hpp

@@ -0,0 +1,38 @@
+#ifndef KALMAN_FILTER_HPP
+#define KALMAN_FILTER_HPP
+#include <sta/math/linalg/matrix.hpp>
+
+namespace math
+{
+
+struct KalmanState
+{
+	matrix error;
+	matrix x;
+};
+
+class KalmanFilter
+{
+private:
+	matrix A_;
+	matrix B_;
+	matrix C_;
+	matrix Q_;
+	matrix R_;
+	uint8_t n_;
+	matrix identity_;
+
+
+
+public:
+	KalmanFilter(matrix, matrix, matrix, matrix, matrix);
+	~KalmanFilter();
+	KalmanState predict(float, KalmanState, matrix);
+	KalmanState correct(float,KalmanState, matrix);
+};
+    
+
+    
+}
+
+#endif // KALMAN_FILTER_HPP