Initial commit

README.md

@@ -1,2 +1,65 @@
-# matrix
-A single-header C++ library for matrix operations, including BLAS and LAPACK support
+# Matrix
+
+Matrix is a single-header C++ matrix library that offers a clean interface for working with matrices in your applications. Notably, this library comes with LAPACK support to provide efficient linear algebra operations, all within a single header file for your convenience.
+
+## Usage
+
+### Including the Header
+
+To use the Matrix class in your C++ project, simply include the single header file:
+
+```cpp
+#include "matrix.h"
+```
+
+If you want to enable BLAS and LAPACK support, you can use the following command after installing [LAPACK](https://github.com/Reference-LAPACK/lapack):
+
+```bash
+make blas=1
+```
+
+### Creating Matrices
+
+Creating a matrix is straightforward:
+
+```cpp
+Matrix<int, 2, 3> matrix;  // Creates a 2x3 matrix of integers, initialized to 0.
+```
+
+You can initialize matrices with data using initializer lists:
+
+```cpp
+Matrix<double, 2, 2> matrix = {1.0, 2.0, 3.0, 4.0};
+```
+
+### Basic Operations
+
+Perform basic operations like addition, subtraction, unary minus, multiplication and inversion:
+
+```cpp
+Matrix<double, 2, 2> sum = matrix1 + matrix2;       // Matrix addition
+Matrix<double, 2, 2> diff = matrix2 - matrix1;      // Matrix subtraction
+Matrix<double, 2, 2> negated = -matrix1;            // Unary minus
+Matrix<double, 2, 2> scaled = matrix1 * 2;          // Scalar multiplication
+Matrix<double, 2, 2> product = matrix1 * matrix2;   // Matrix multiplication
+Matrix<double, 2, 2> inverse = matrix1.inv();       // Matrix inversion
+Matrix<double, 2, 2> transposed = matrix1.t()       // Matrix Transposition
+```
+
+### Accessing Elements
+
+Access matrix elements using parentheses or `data()` for const objects:
+
+```cpp
+double element = matrix(0, 1);  // Accesses the element at row 0, column 1
+const double* dataPtr = matrix.data();
+double thirdElement = dataPtr[2];  // Accesses the third element
+```
+
+### Printing Matrices
+
+Print matrices neatly using the overloaded `<<` operator:
+
+```cpp
+std::cout << "Matrix:\n" << matrix << std::endl;
+```

example/kalman/kalman_filter.cpp

@@ -0,0 +1,74 @@
+#include "kalman_filter.h"
+
+KalmanFilter::KalmanFilter(
+    const Matrix<double, 8, 1>& initialState,
+    const Matrix<double, 8, 8>& initialCovariance,
+    const Matrix<double, 8, 8>& initialTransition,
+    const Matrix<double, 6, 8>& initialMeasurement,
+    const Matrix<double, 6, 6>& initialMeasurementNoiseCov,
+    const Matrix<double, 8, 8>& initialProcessNoiseCov)
+    : state_(initialState),
+      covariance_(initialCovariance),
+      transitionMatrix_(initialTransition),
+      measurementMatrix_(initialMeasurement),
+      measurementNoiseCov_(initialMeasurementNoiseCov),
+      processNoiseCov_(initialProcessNoiseCov) {}
+
+Matrix<double, 8, 1> KalmanFilter::getState() const { return state_; }
+
+void KalmanFilter::predict(const Matrix<double, 6, 1>& measurement,
+                           const double dt) {
+  transitionMatrix_(0, 1) = dt;
+  transitionMatrix_(1, 2) = dt;
+  transitionMatrix_(3, 4) = dt;
+  transitionMatrix_(4, 5) = dt;
+  transitionMatrix_(6, 7) = dt;
+  state_ = transitionMatrix_ * state_;
+  covariance_ = transitionMatrix_ * covariance_ * transitionMatrix_.t() +
+                processNoiseCov_;
+  Matrix<double, 6, 1> innovation = measurement - measurementMatrix_ * state_;
+  Matrix<double, 6, 6> innovationCov =
+      measurementMatrix_ * covariance_ * measurementMatrix_.t() +
+      measurementNoiseCov_;
+  Matrix<double, 8, 6> kalmanGain =
+      covariance_ * measurementMatrix_.t() * innovationCov.inv();
+  state_ = state_ + kalmanGain * innovation;
+  covariance_ = covariance_ - kalmanGain * measurementMatrix_ * covariance_;
+}
+
+int main() {
+  // Define initial matrices for Kalman filter
+  Matrix<double, 8, 1> initialState{
+      0, 12, 0.1, 0, 0.2, 0.1, 3.14, 0.12,
+  };
+  Eye<double, 8> initialCovariance;
+  Eye<double, 8> initialTransition;
+  Matrix<double, 6, 8> initialMeasurement;
+  initialMeasurement(0, 1) = 1;
+  initialMeasurement(1, 2) = 1;
+  initialMeasurement(2, 4) = 1;
+  initialMeasurement(3, 5) = 1;
+  initialMeasurement(4, 6) = 1;
+  initialMeasurement(5, 7) = 1;
+  Eye<double, 6> initialMeasurementNoiseCov;
+  initialMeasurementNoiseCov *= 0.1;
+  Eye<double, 8> initialProcessNoiseCov;
+  initialProcessNoiseCov *= 0.01;
+
+  // Initialize the Kalman filter
+  KalmanFilter kf(initialState, initialCovariance, initialTransition,
+                  initialMeasurement, initialMeasurementNoiseCov,
+                  initialProcessNoiseCov);
+
+  // Perform Kalman filter operations
+  Matrix<double, 6, 1> measurement{
+      12.0, 0.2, 0.1, 0.1, 3.141, 0.11,
+  };
+  double dt = 0.15;
+
+  kf.predict(measurement, dt);
+
+  std::cout << kf.getState() << std::endl;
+
+  return 0;
+}

example/kalman/kalman_filter.h

@@ -0,0 +1,28 @@
+#ifndef KALMAN_FILTER_H
+#define KALMAN_FILTER_H
+
+#include "../../matrix.h"
+
+class KalmanFilter {
+ private:
+  Matrix<double, 8, 1> state_;                // Current state_ estimate
+  Matrix<double, 8, 8> covariance_;           // Error covariance_ matrix
+  Matrix<double, 8, 8> transitionMatrix_;     // State transition matrix
+  Matrix<double, 6, 8> measurementMatrix_;    // Measurement matrix
+  Matrix<double, 6, 6> measurementNoiseCov_;  // Measurement noise covariance
+  Matrix<double, 8, 8> processNoiseCov_;      // Process noise covariance
+
+ public:
+  KalmanFilter(const Matrix<double, 8, 1>& initialState,
+               const Matrix<double, 8, 8>& initialCovariance,
+               const Matrix<double, 8, 8>& initialTransition,
+               const Matrix<double, 6, 8>& initialMeasurement,
+               const Matrix<double, 6, 6>& initialMeasurementNoiseCov,
+               const Matrix<double, 8, 8>& initialProcessNoiseCov);
+
+  Matrix<double, 8, 1> getState() const;
+
+  void predict(const Matrix<double, 6, 1>& measurement, const double dt);
+};
+
+#endif  // KALMAN_FILTER_HPP

example/kalman/kalman_filter.py

@@ -0,0 +1,90 @@
+import numpy as np
+
+
+def kalman_filter(x, P, F, H, R, Q, measurement, dt):
+    # Update state transition matrix based on dynamic time step
+    F[0, 1] = dt
+    F[1, 2] = dt
+    F[3, 4] = dt
+    F[4, 5] = dt
+    F[6, 7] = dt
+
+    # Prediction step
+    x_predicted = np.dot(F, x)
+    P_predicted = np.dot(np.dot(F, P), F.T) + Q
+
+    # Update step
+    y = measurement - np.dot(H, x_predicted)
+    S = np.dot(np.dot(H, P_predicted), H.T) + R
+    K = np.dot(np.dot(P_predicted, H.T), np.linalg.inv(S))
+    x = x_predicted + np.dot(K, y)
+    P = np.dot((np.eye(x.shape[0]) - np.dot(K, H)), P_predicted)
+    return x, P
+
+
+# Initial error covariance matrix
+P = np.eye(8)
+
+# Define the state transition matrix
+F = np.array(
+    [
+        [1, 1, 0, 0, 0, 0, 0, 0],
+        [0, 1, 1, 0, 0, 0, 0, 0],
+        [0, 0, 1, 0, 0, 0, 0, 0],
+        [0, 0, 0, 1, 1, 0, 0, 0],
+        [0, 0, 0, 0, 1, 1, 0, 0],
+        [0, 0, 0, 0, 0, 1, 0, 0],
+        [0, 0, 0, 0, 0, 0, 1, 1],
+        [0, 0, 0, 0, 0, 0, 0, 1],
+    ],
+    dtype=float,
+)
+
+# Define the measurement matrix (speed, acceleration, yaw, yaw rate)
+H = np.array(
+    [
+        [0, 1, 0, 0, 0, 0, 0, 0],
+        [0, 0, 1, 0, 0, 0, 0, 0],
+        [0, 0, 0, 0, 1, 0, 0, 0],
+        [0, 0, 0, 0, 0, 1, 0, 0],
+        [0, 0, 0, 0, 0, 0, 1, 0],
+        [0, 0, 0, 0, 0, 0, 0, 1],
+    ]
+)
+
+# Define measurement noise covariance
+R = np.eye(6) * 0.1
+
+# Define process noise covariance
+Q = np.eye(8) * 0.01
+
+kalman_state = np.array(
+    [
+        0,
+        12,
+        0.1,
+        0,
+        0.2,
+        0.1,
+        3.14,
+        0.12,
+    ]
+).T  # Initial x, vx, ax, y, vy, ay, yaw, yaw rate
+
+measurement = np.array(
+    [
+        12.0,
+        0.2,
+        0.1,
+        0.1,
+        3.141,
+        0.11,
+        0.15,
+    ]
+).T
+
+kalman_state, P = kalman_filter(
+    kalman_state, P, F, H, R, Q, measurement[:-1], measurement[-1]
+)
+
+print(kalman_state)

example/kalman/makefile

@@ -0,0 +1,20 @@
+# [email protected]:Reference-LAPACK/lapack.git
+LAPACK 		= /path/to/lapack
+
+CXX			= g++
+CXXFLAG		= -w -std=c++11 -g
+INCLUDE		= -I.
+SRC			= $(wildcard ./*.cpp)
+
+ifdef blas
+CXXFLAG		+= -DBLAS
+LDFLAG		+= -L$(LAPACK) -llapacke -llapack -lcblas  -lrefblas  -lm -lgfortran
+INCLUDE		+= -I$(LAPACK)/LAPACKE/include -I$(LAPACK)/CBLAS/include
+endif
+
+kalman: $(SRC)
+	@$(CXX) $(CXXFLAG) $(INCLUDE) $(SRC) $(LDFLAG) -o kalman
+
+.PHONY: clean
+clean:
+	@rm kalman

example/tutorial/makefile

@@ -0,0 +1,20 @@
+# [email protected]:Reference-LAPACK/lapack.git
+LAPACK 		= /path/to/lapack
+
+CXX			= g++
+CXXFLAG		= -w -std=c++11 -g
+INCLUDE		= -I.
+SRC			= $(wildcard ./*.cpp)
+
+ifdef blas
+CXXFLAG		+= -DBLAS
+LDFLAG		+= -L$(LAPACK) -llapacke -llapack -lcblas  -lrefblas  -lm -lgfortran
+INCLUDE		+= -I$(LAPACK)/LAPACKE/include -I$(LAPACK)/CBLAS/include
+endif
+
+tutorial: $(SRC)
+	@$(CXX) $(CXXFLAG) $(INCLUDE) $(SRC) $(LDFLAG) -o tutorial
+
+.PHONY: clean
+clean:
+	@rm tutorial

example/tutorial/tutorial.cpp

@@ -0,0 +1,62 @@
+#include <iostream>
+#include <vector>
+
+#include "../../matrix.h"
+
+int main() {
+  // Creating Matrices
+  // -----------------
+
+  // Create a 2x2 matrix with double values, initialized to 0
+  Matrix<double, 2, 2> mat1;
+
+  // Create a 2x2 matrix with doubleeger values using an initializer list
+  Matrix<double, 2, 2> mat2 = {1, 2, 3, 4};
+
+  // Basic Matrix Operations
+  // ------------------------
+
+  // Matrix addition
+  Matrix<double, 2, 2> sum = mat1 + mat2;
+
+  // Matrix subtraction
+  Matrix<double, 2, 2> diff = mat2 - mat1;
+
+  // Matrix unary minus
+  Matrix<double, 2, 2> negated = -mat2;
+
+  // Matrix-scalar multiplication
+  Matrix<double, 2, 2> scaled = mat2 * 2;
+
+  // Matrix-scalar multiplication assignment
+  mat2 *= 3;
+
+  // Transpose a matrix
+  Matrix<double, 2, 2> transposed = mat2.t();
+
+  // Matrix-Matrix Multiplication
+  Matrix<double, 2, 2> mat3 = {1, 2, 3, 4};
+  Matrix<double, 2, 2> result3 = mat3 * mat2;
+
+  // Matrix Inversion
+  Matrix<double, 2, 2> inverted = mat3.inv();
+
+  // Accessing Matrix Elements
+  // -------------------------
+
+  // Access elements using parentheses
+  double element = mat2(0, 1);  // Accesses the element at row 0, column 1
+
+  // Access elements using data()
+  double* dataPtr = mat2.data();
+  double thirdElement = dataPtr[2];
+
+  // Printing a Matrix
+  // -----------------
+
+  // You can use the overloaded << operator to print a matrix
+  std::cout << "Matrix mat2:\n" << mat2 << std::endl;
+
+
+  return 0;
+}

makefile

@@ -0,0 +1,20 @@
+# [email protected]:Reference-LAPACK/lapack.git
+LAPACK 		= /path/to/lapack
+
+CXX			= g++
+CXXFLAG		= -w -std=c++11 -g
+INCLUDE		= -I.
+SRC			= $(wildcard ./*.cpp)
+
+ifdef blas
+CXXFLAG		+= -DBLAS
+LDFLAG		+= -L$(LAPACK) -llapacke -llapack -lcblas  -lrefblas  -lm -lgfortran
+INCLUDE		+= -I$(LAPACK)/LAPACKE/include -I$(LAPACK)/CBLAS/include
+endif
+
+demo: $(SRC)
+	@$(CXX) $(CXXFLAG) $(INCLUDE) $(SRC) $(LDFLAG) -o demo
+
+.PHONY: clean
+clean:
+	@rm demo