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

include/sta/math/linalg/linalg.hpp

@@ -0,0 +1,32 @@
+#ifndef INC_LINALG_HPP_
+#define INC_LINALG_HPP_
+#include <sta/math/linalg/matrix.hpp>
+
+namespace math 
+{
+namespace linalg 
+{
+
+matrix dot(matrix, matrix);
+float norm(matrix);
+matrix normalize(matrix);
+matrix cross(matrix, matrix);
+matrix skew_symmetric(matrix);
+matrix add(matrix, matrix);
+matrix subtract(matrix, matrix);
+matrix dot(matrix, float);
+matrix cof(matrix);
+matrix adj(matrix);
+
+matrix inv(matrix);
+
+matrix inv_adj(matrix);
+matrix inv_char_poly(matrix);
+matrix inv_schur_dec(matrix);
+
+matrix _inv_char_poly_3x3(matrix);
+matrix _inv_char_poly_2x2(matrix);
+
+}
+}
+#endif /* INC_LINALG_HPP_ */

include/sta/math/linalg/matrix.hpp

@@ -0,0 +1,60 @@
+#ifndef INC_MATRIX_HPP_
+#define INC_MATRIX_HPP_
+#include <cstdint>
+
+namespace math
+{
+    
+struct matrix 
+{
+
+    float * datafield = nullptr;
+    uint8_t * shape = nullptr;
+
+    matrix();
+    matrix(const matrix&);
+    matrix(uint8_t, uint8_t);
+    matrix(uint8_t, uint8_t, float*);
+    ~matrix();
+
+    bool is_valid();
+
+    uint16_t get_size();
+    uint8_t get_rows();
+    uint8_t get_cols();
+
+    matrix clone();
+    void show_serial();
+    void show_shape();
+
+    matrix& operator=(matrix);
+    void reshape(uint8_t, uint8_t);
+    
+    float det();
+    matrix get_block(uint8_t, uint8_t, uint8_t, uint8_t);
+    void set_block(uint8_t, uint8_t, matrix);
+    void set(uint8_t, uint8_t, float);
+    void set(uint16_t, float);
+    matrix get_submatrix(uint8_t, uint8_t);
+
+    static matrix eye(uint8_t);
+    static matrix zeros(uint8_t, uint8_t);
+    
+    float operator()(uint8_t, uint8_t);
+    float operator[](uint16_t);
+    uint16_t get_idx(uint8_t, uint8_t);
+
+    matrix T();
+    matrix flatten();
+    float minor(uint8_t, uint8_t);
+
+    matrix operator*(float);
+    matrix operator*(matrix);
+    matrix operator+(matrix);
+    matrix operator-(matrix);
+
+};
+
+
+}
+#endif /* INC_MATRIX_HPP_ */

include/sta/math/utils.hpp

@@ -0,0 +1,10 @@
+#ifndef INC_UTILS_HPP_
+#define INC_UTILS_HPP_
+namespace math
+{
+    float fast_inv_sqrt(float);
+        
+    
+} // namespace stamath
+
+#endif /* INC_UTILS_HPP_ */