diff --git a/sbs.conf b/sbs.conf
index 9a20ca86643e812f597d8a667d158b4adb178f9d..1bc18d5bd1b64c225b4da538fc7a34a931d2e05d 100644
--- a/sbs.conf
+++ b/sbs.conf
@@ -473,4 +473,12 @@ BoardId: stm32f429zi_stm32f4discovery
BinName: test-l3gd20
Include: %shared %spi
Defines:
-Main: drivers/test-l3gd20
\ No newline at end of file
+Main: drivers/test-l3gd20
+
+[test-rls]
+Type: test
+BoardId: stm32f429zi_skyward_death_stack
+BinName: test-rls
+Include: %shared
+Defines:
+Main: test-rls
diff --git a/src/shared/rls/RLS.h b/src/shared/rls/RLS.h
new file mode 100644
index 0000000000000000000000000000000000000000..1e41e839bd18a67dfbc536c9a8049d8c539f082d
--- /dev/null
+++ b/src/shared/rls/RLS.h
@@ -0,0 +1,65 @@
+/* Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Luca Mozzarelli
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <../libs/simple-template-matrix/matrix.h>
+#include <iostream>
+
+/*!
+ * \class RLS
+ * \brief A performin Recursive least squares identification
+ *
+ * ``` n ```: number of parameters to be estimated
+ * ``` n ```: number of states
+ * ``` p ```: number of outputs
+ *
+ * The system model is:
+ * ```
+ * y(t) = phi(t)^T * theta(t)
+ * ```
+ * being y the output and theta the vector of parameters to be identified.
+ * See test-rls.cpp for an example.
+ */
+template <unsigned n>
+class RLS
+{
+ using CVectorN = MatrixBase<float, n, 1>;
+ using MatrixNN = MatrixBase<float, n, n>;
+private:
+ MatrixNN V;
+ CVectorN theta;
+ float mu;
+public:
+ RLS(const MatrixNN &V_0, const CVectorN &theta_0, float mu) : V(V_0), theta(theta_0), mu(mu){};
+
+
+ ~RLS(){};
+
+ void update(const CVectorN & y_kp1, const CVectorN & phi)
+ {
+ V = V/mu - (V/mu * phi*transpose(phi) * V/mu) / (1 + transpose(phi)*V/mu* phi)(0,0);
+ theta = theta + V*phi*(y_kp1 - transpose(theta)*phi);
+ };
+
+ CVectorN getEstimate(){ return theta; };
+};
\ No newline at end of file
diff --git a/src/tests/test-rls.cpp b/src/tests/test-rls.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f0973ebaa8719de5f59d0b9da9084f38277745d6
--- /dev/null
+++ b/src/tests/test-rls.cpp
@@ -0,0 +1,88 @@
+/* Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Luca Mozzarelli
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+/*
+* This test simulates power control in an unknown resistor (i.e. thermal cutters ;) )
+* y(t) = phi(t)^T * theta(t)
+* P = V^2/R
+*
+* y(t) = P(t) = V(t)*I(t) (power on the resistor)
+* phi(t) = V(t)^2 (square of the voltage on the resistor)
+* theta(t) = 1/R (inverse of the resistance)
+*/
+
+#include <libs/simple-template-matrix/matrix.h>
+#include <rls/RLS.h>
+#include <math.h>
+#include <iostream>
+
+
+int main()
+{
+
+ float R = 0.3; // True resistance
+ float P_ref = 20; // Power setpoint
+ float V; // Voltage applied to the resistor
+ float I; // Current in the resistor
+
+ float R0 = 0.5; // Inital guess of R
+ MatrixBase<float,1,1> theta0{1/R0};
+ MatrixBase<float,1,1> V0{1}; // Confidence of the initial guess (variance)
+ float mu = 0.7; // Forgetting factor
+ float R_est; // Estimated R
+
+ // Instantiate the RLS
+ RLS<1> rls{V0,theta0,mu};
+
+ MatrixBase<float,1,1> phi{0};
+ MatrixBase<float,1,1> y{0};
+
+ for (unsigned i = 0; i < 1000; i++)
+ {
+ // Get the estimate of R
+ R_est = 1/(rls.getEstimate()(0,0));
+
+ // Compute the power on the resistor
+ V = sqrtf(P_ref*R_est);
+
+ // Compute the mesured current
+ I = V/R;
+
+ // Regressors vector
+ phi(0,0) = V*V;
+
+ // Measurement vector
+ y(0,0) = V*I;
+
+ // Update the filter
+ rls.update(y,phi);
+
+ // std::cout << R << ", " << R_est << ", " << R-R_est << ", " << V*I << "\n";
+
+ // Start a slope on R
+ if (i > 500)
+ {
+ R = R+0.05;
+ }
+ }
+
+}
\ No newline at end of file