tempcal: move to using Matrix library functions

src/modules/tempcal/CMakeLists.txt

@@ -38,8 +38,6 @@ px4_add_module(
 	STACK_MAX 4000
 	SRCS
 	tempcal_main.cpp
-	polyfit.cpp
-	matrix_alg.cpp
 	DEPENDS
 	platforms__common
 )

src/modules/tempcal/matrix_alg.cpp

@@ -1,394 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Siddharth Bharat Purohit <sidbpurohit@gmail.com>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file matrix_alg.cpp
- *
- * Matrix algebra on raw arrays
- */
-
-#include "matrix_alg.h"
-#include <stdio.h>
-/*
- *    Does matrix multiplication of two regular/square matrices
- *
- *    @param     A,           Matrix A
- *    @param     B,           Matrix B
- *    @param     n,           dimemsion of square matrices
- *    @returns                multiplied matrix i.e. A*B
- */
-
-float *mat_mul(float *A, float *B, uint8_t n)
-{
-	float *ret = new float[n * n];
-	memset(ret, 0.0f, n * n * sizeof(float));
-
-	for (uint8_t i = 0; i < n; i++) {
-		for (uint8_t j = 0; j < n; j++) {
-			for (uint8_t k = 0; k < n; k++) {
-				ret[i * n + j] += A[i * n + k] * B[k * n + j];
-			}
-		}
-	}
-
-	return ret;
-}
-
-static inline void swap(float &a, float &b)
-{
-	float c;
-	c = a;
-	a = b;
-	b = c;
-}
-
-/*
- *    calculates pivot matrix such that all the larger elements in the row are on diagonal
- *
- *    @param     A,           input matrix matrix
- *    @param     pivot
- *    @param     n,           dimenstion of square matrix
- *    @returns                false = matrix is Singular or non positive definite, true = matrix inversion successful
- */
-
-static void mat_pivot(float *A, float *pivot, uint8_t n)
-{
-	for (uint8_t i = 0; i < n; i++) {
-		for (uint8_t j = 0; j < n; j++) {
-			pivot[i * n + j] = (i == j);
-		}
-	}
-
-	for (uint8_t i = 0; i < n; i++) {
-		uint8_t max_j = i;
-
-		for (uint8_t j = i; j < n; j++) {
-			if (fabsf(A[j * n + i]) > fabsf(A[max_j * n + i])) {
-				max_j = j;
-			}
-		}
-
-		if (max_j != i) {
-			for (uint8_t k = 0; k < n; k++) {
-				swap(pivot[i * n + k], pivot[max_j * n + k]);
-			}
-		}
-	}
-}
-
-/*
- *    calculates matrix inverse of Lower trangular matrix using forward substitution
- *
- *    @param     L,           lower triangular matrix
- *    @param     out,         Output inverted lower triangular matrix
- *    @param     n,           dimension of matrix
- */
-
-static void mat_forward_sub(float *L, float *out, uint8_t n)
-{
-	// Forward substitution solve LY = I
-	for (int i = 0; i < n; i++) {
-		out[i * n + i] = 1 / L[i * n + i];
-
-		for (int j = i + 1; j < n; j++) {
-			for (int k = i; k < j; k++) {
-				out[j * n + i] -= L[j * n + k] * out[k * n + i];
-			}
-
-			out[j * n + i] /= L[j * n + j];
-		}
-	}
-}
-
-/*
- *    calculates matrix inverse of Upper trangular matrix using backward substitution
- *
- *    @param     U,           upper triangular matrix
- *    @param     out,         Output inverted upper triangular matrix
- *    @param     n,           dimension of matrix
- */
-
-static void mat_back_sub(float *U, float *out, uint8_t n)
-{
-	// Backward Substitution solve UY = I
-	for (int i = n - 1; i >= 0; i--) {
-		out[i * n + i] = 1 / U[i * n + i];
-
-		for (int j = i - 1; j >= 0; j--) {
-			for (int k = i; k > j; k--) {
-				out[j * n + i] -= U[j * n + k] * out[k * n + i];
-			}
-
-			out[j * n + i] /= U[j * n + j];
-		}
-	}
-}
-
-/*
- *    Decomposes square matrix into Lower and Upper triangular matrices such that
- *    A*P = L*U, where P is the pivot matrix
- *    ref: http://rosettacode.org/wiki/LU_decomposition
- *    @param     U,           upper triangular matrix
- *    @param     out,         Output inverted upper triangular matrix
- *    @param     n,           dimension of matrix
- */
-
-static void mat_LU_decompose(float *A, float *L, float *U, float *P, uint8_t n)
-{
-	memset(L, 0, n * n * sizeof(float));
-	memset(U, 0, n * n * sizeof(float));
-	memset(P, 0, n * n * sizeof(float));
-	mat_pivot(A, P, n);
-
-	float *APrime = mat_mul(P, A, n);
-
-	for (uint8_t i = 0; i < n; i++) {
-		L[i * n + i] = 1;
-	}
-
-	for (uint8_t i = 0; i < n; i++) {
-		for (uint8_t j = 0; j < n; j++) {
-			if (j <= i) {
-				U[j * n + i] = APrime[j * n + i];
-
-				for (uint8_t k = 0; k < j; k++) {
-					U[j * n + i] -= L[j * n + k] * U[k * n + i];
-				}
-			}
-
-			if (j >= i) {
-				L[j * n + i] = APrime[j * n + i];
-
-				for (uint8_t k = 0; k < i; k++) {
-					L[j * n + i] -= L[j * n + k] * U[k * n + i];
-				}
-
-				L[j * n + i] /= U[i * n + i];
-			}
-		}
-	}
-
-	delete[] APrime;
-}
-
-/*
- *    matrix inverse code for any square matrix using LU decomposition
- *    inv = inv(U)*inv(L)*P, where L and U are triagular matrices and P the pivot matrix
- *    ref: http://www.cl.cam.ac.uk/teaching/1314/NumMethods/supporting/mcmaster-kiruba-ludecomp.pdf
- *    @param     m,           input 4x4 matrix
- *    @param     inv,      Output inverted 4x4 matrix
- *    @param     n,           dimension of square matrix
- *    @returns                false = matrix is Singular, true = matrix inversion successful
- */
-bool mat_inverse(float *A, float *inv, uint8_t n)
-{
-	float *L, *U, *P;
-	bool ret = true;
-	L = new float[n * n];
-	U = new float[n * n];
-	P = new float[n * n];
-	mat_LU_decompose(A, L, U, P, n);
-
-	float *L_inv = new float[n * n];
-	float *U_inv = new float[n * n];
-
-	memset(L_inv, 0, n * n * sizeof(float));
-	mat_forward_sub(L, L_inv, n);
-
-	memset(U_inv, 0, n * n * sizeof(float));
-	mat_back_sub(U, U_inv, n);
-
-	// decomposed matrices no longer required
-	delete[] L;
-	delete[] U;
-
-	float *inv_unpivoted = mat_mul(U_inv, L_inv, n);
-	float *inv_pivoted = mat_mul(inv_unpivoted, P, n);
-
-	//check sanity of results
-	for (uint8_t i = 0; i < n; i++) {
-		for (uint8_t j = 0; j < n; j++) {
-			if (isnan(inv_pivoted[i * n + j]) || isinf(inv_pivoted[i * n + j])) {
-				ret = false;
-			}
-		}
-	}
-
-	memcpy(inv, inv_pivoted, n * n * sizeof(float));
-
-	//free memory
-	delete[] inv_pivoted;
-	delete[] inv_unpivoted;
-	delete[] P;
-	delete[] U_inv;
-	delete[] L_inv;
-	return ret;
-}
-
-bool inverse4x4(double m[], double invOut[])
-{
-	double inv[16], det;
-	uint8_t i;
-
-	inv[0] = m[5]  * m[10] * m[15] -
-		 m[5]  * m[11] * m[14] -
-		 m[9]  * m[6]  * m[15] +
-		 m[9]  * m[7]  * m[14] +
-		 m[13] * m[6]  * m[11] -
-		 m[13] * m[7]  * m[10];
-
-	inv[4] = -m[4]  * m[10] * m[15] +
-		 m[4]  * m[11] * m[14] +
-		 m[8]  * m[6]  * m[15] -
-		 m[8]  * m[7]  * m[14] -
-		 m[12] * m[6]  * m[11] +
-		 m[12] * m[7]  * m[10];
-
-	inv[8] = m[4]  * m[9] * m[15] -
-		 m[4]  * m[11] * m[13] -
-		 m[8]  * m[5] * m[15] +
-		 m[8]  * m[7] * m[13] +
-		 m[12] * m[5] * m[11] -
-		 m[12] * m[7] * m[9];
-
-	inv[12] = -m[4]  * m[9] * m[14] +
-		  m[4]  * m[10] * m[13] +
-		  m[8]  * m[5] * m[14] -
-		  m[8]  * m[6] * m[13] -
-		  m[12] * m[5] * m[10] +
-		  m[12] * m[6] * m[9];
-
-	inv[1] = -m[1]  * m[10] * m[15] +
-		 m[1]  * m[11] * m[14] +
-		 m[9]  * m[2] * m[15] -
-		 m[9]  * m[3] * m[14] -
-		 m[13] * m[2] * m[11] +
-		 m[13] * m[3] * m[10];
-
-	inv[5] = m[0]  * m[10] * m[15] -
-		 m[0]  * m[11] * m[14] -
-		 m[8]  * m[2] * m[15] +
-		 m[8]  * m[3] * m[14] +
-		 m[12] * m[2] * m[11] -
-		 m[12] * m[3] * m[10];
-
-	inv[9] = -m[0]  * m[9] * m[15] +
-		 m[0]  * m[11] * m[13] +
-		 m[8]  * m[1] * m[15] -
-		 m[8]  * m[3] * m[13] -
-		 m[12] * m[1] * m[11] +
-		 m[12] * m[3] * m[9];
-
-	inv[13] = m[0]  * m[9] * m[14] -
-		  m[0]  * m[10] * m[13] -
-		  m[8]  * m[1] * m[14] +
-		  m[8]  * m[2] * m[13] +
-		  m[12] * m[1] * m[10] -
-		  m[12] * m[2] * m[9];
-
-	inv[2] = m[1]  * m[6] * m[15] -
-		 m[1]  * m[7] * m[14] -
-		 m[5]  * m[2] * m[15] +
-		 m[5]  * m[3] * m[14] +
-		 m[13] * m[2] * m[7] -
-		 m[13] * m[3] * m[6];
-
-	inv[6] = -m[0]  * m[6] * m[15] +
-		 m[0]  * m[7] * m[14] +
-		 m[4]  * m[2] * m[15] -
-		 m[4]  * m[3] * m[14] -
-		 m[12] * m[2] * m[7] +
-		 m[12] * m[3] * m[6];
-
-	inv[10] = m[0]  * m[5] * m[15] -
-		  m[0]  * m[7] * m[13] -
-		  m[4]  * m[1] * m[15] +
-		  m[4]  * m[3] * m[13] +
-		  m[12] * m[1] * m[7] -
-		  m[12] * m[3] * m[5];
-
-	inv[14] = -m[0]  * m[5] * m[14] +
-		  m[0]  * m[6] * m[13] +
-		  m[4]  * m[1] * m[14] -
-		  m[4]  * m[2] * m[13] -
-		  m[12] * m[1] * m[6] +
-		  m[12] * m[2] * m[5];
-
-	inv[3] = -m[1] * m[6] * m[11] +
-		 m[1] * m[7] * m[10] +
-		 m[5] * m[2] * m[11] -
-		 m[5] * m[3] * m[10] -
-		 m[9] * m[2] * m[7] +
-		 m[9] * m[3] * m[6];
-
-	inv[7] = m[0] * m[6] * m[11] -
-		 m[0] * m[7] * m[10] -
-		 m[4] * m[2] * m[11] +
-		 m[4] * m[3] * m[10] +
-		 m[8] * m[2] * m[7] -
-		 m[8] * m[3] * m[6];
-
-	inv[11] = -m[0] * m[5] * m[11] +
-		  m[0] * m[7] * m[9] +
-		  m[4] * m[1] * m[11] -
-		  m[4] * m[3] * m[9] -
-		  m[8] * m[1] * m[7] +
-		  m[8] * m[3] * m[5];
-
-	inv[15] = m[0] * m[5] * m[10] -
-		  m[0] * m[6] * m[9] -
-		  m[4] * m[1] * m[10] +
-		  m[4] * m[2] * m[9] +
-		  m[8] * m[1] * m[6] -
-		  m[8] * m[2] * m[5];
-
-	det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
-
-	if (fabsf(det) < 1.1755e-38f) {
-		return false;
-	}
-
-	det = 1.0 / det;
-
-	for (i = 0; i < 16; i++) {
-		invOut[i] = inv[i] * det;
-	}
-
-	return true;
-}
-
-bool inverse(float *A, float *inv, uint8_t n)
-{
-	return mat_inverse(A, inv, n);
-}

src/modules/tempcal/matrix_alg.h

@@ -1,51 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Siddharth Bharat Purohit <sibpurohit@gmail.com>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file matrix_alg.h
- *
- * Matrix algebra on raw arrays
- */
-
-
-#pragma once
-
-#include <inttypes.h>
-#include <string.h>
-#include <math.h>
-
-float *mat_mul(float *A, float *B, uint8_t n);
-bool mat_inverse(float *A, float *inv, uint8_t n);
-bool inverse4x4(double m[], double invOut[]);
-bool inverse(float *A, float *inv, uint8_t n);

src/modules/tempcal/polyfit.h

@@ -1,77 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2015-2016 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-/*
-Polygon linear fit
-Author: Siddharth Bharat Purohit
-*/
-#pragma once
-#include <px4_config.h>
-#include <px4_defines.h>
-#include <px4_tasks.h>
-#include <px4_posix.h>
-#include <px4_time.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-#include <poll.h>
-#include <time.h>
-#include <float.h>
-
-class polyfitter
-{
-public:
-	polyfitter() {}
-	~polyfitter()
-	{
-		if (VTV != nullptr) {
-			delete[] VTV;
-		}
-
-		if (VTY != nullptr) {
-			delete[] VTY;
-		}
-	}
-	void update(double x, double y);
-	int init(uint8_t order);
-	bool fit(double res[]);
-private:
-	double *VTV;
-	double *VTY;
-	uint8_t _forder;
-	void update_VTV(double x);
-	void update_VTY(double x, double y);
-};

src/modules/tempcal/polyfit.hpp

@@ -34,8 +34,24 @@
 Polygon linear fit
 Author: Siddharth Bharat Purohit
 */
-#include "polyfit.h"
+
-#include "matrix_alg.h"
+#pragma once
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_tasks.h>
+#include <px4_posix.h>
+#include <px4_time.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <time.h>
+#include <float.h>
+#include <matrix/math.hpp>
 
 #define DEBUG 0
 #if DEBUG
@@ -44,91 +60,28 @@ Author: Siddharth Bharat Purohit
 #define PF_DEBUG(fmt, ...)
 #endif
 
-int polyfitter::init(uint8_t order)
+template<size_t _forder>
+class polyfitter
 {
-	_forder = order + 1;
+public:
-	VTV = new double[_forder * _forder];
+	polyfitter() {}
 
-	if (VTV == NULL) {
+	void update(double x, double y)
-		return -1;
+	{
+		update_VTV(x);
+		update_VTY(x, y);
 	}
 
-	VTY = new double[_forder];
+	bool fit(double res[])
+	{
+		//Do inverse of VTV
+		matrix::SquareMatrix<double, _forder> IVTV;
 
-	if (VTY == NULL) {
+		IVTV = VTV.I();
-		return -1;
-	}
 
-	memset(VTV, 0, sizeof(double)*_forder * _forder);
-	memset(VTY, 0, sizeof(double)*_forder);
-	return 0;
-}
-
-void polyfitter::update(double x, double y)
-{
-	update_VTV(x);
-	update_VTY(x, y);
-}
-
-void polyfitter::update_VTY(double x, double y)
-{
-	double temp = 1.0f;
-	PF_DEBUG("O %.6f\n", (double)x);
-
-	for (int8_t i = _forder - 1; i >= 0; i--) {
-		VTY[i] += y * temp;
-		temp *= x;
-		PF_DEBUG("%.6f ", (double)VTY[i]);
-	}
-
-	PF_DEBUG("\n");
-}
-
-
-void polyfitter::update_VTV(double x)
-{
-	double temp = 1.0f;
-	int8_t z;
-
-	for (uint8_t i = 0; i < _forder; i++) {
-		for (int j = 0; j < _forder; j++) {
-			PF_DEBUG("%.10f ", (double)VTV[i * _forder + j]);
-		}
-
-		PF_DEBUG("\n");
-	}
-
-	for (int8_t i = 2 * _forder - 2; i >= 0; i--) {
-		if (i < _forder) {
-			z = 0.0f;
-
-		} else {
-			z = i - _forder + 1;
-		}
-
-		for (int8_t j = i - z; j >= z; j--) {
-			uint8_t row = j;
-			uint8_t col = i - j;
-			VTV[row * _forder  + col] += (double)temp;
-		}
-
-		temp *= x;
-	}
-}
-
-bool polyfitter::fit(double res[])
-{
-	//Do inverse of VTV
-	double *IVTV = new double[_forder * _forder];
-
-	if (VTV == NULL) {
-		return false;
-	}
-
-	if (inverse4x4(VTV, IVTV)) {
 		for (uint8_t i = 0; i < _forder; i++) {
 			for (int j = 0; j < _forder; j++) {
-				PF_DEBUG("%.10f ", (double)IVTV[i * _forder + j]);
+				PF_DEBUG("%.10f ", (double)IVTV(i, j));
 			}
 
 			PF_DEBUG("\n");
@@ -138,7 +91,7 @@ bool polyfitter::fit(double res[])
 			res[i] = 0.0f;
 
 			for (int j = 0; j < _forder; j++) {
-				res[i] += IVTV[i * _forder + j] * (double)VTY[j];
+				res[i] += IVTV(i, j) * (double)VTY(j);
 			}
 
 			PF_DEBUG("%.10f ", res[i]);
@@ -147,5 +100,52 @@ bool polyfitter::fit(double res[])
 		return true;
 	}
 
-	return false;
+private:
-}
+	matrix::SquareMatrix<double, _forder> VTV;
+	matrix::Vector<double, _forder> VTY;
+
+	void update_VTY(double x, double y)
+	{
+		double temp = 1.0f;
+		PF_DEBUG("O %.6f\n", (double)x);
+
+		for (int8_t i = _forder - 1; i >= 0; i--) {
+			VTY(i) += y * temp;
+			temp *= x;
+			PF_DEBUG("%.6f ", (double)VTY(i));
+		}
+
+		PF_DEBUG("\n");
+	}
+
+	void update_VTV(double x)
+	{
+		double temp = 1.0f;
+		int8_t z;
+
+		for (uint8_t i = 0; i < _forder; i++) {
+			for (int j = 0; j < _forder; j++) {
+				PF_DEBUG("%.10f ", (double)VTV(i, j));
+			}
+
+			PF_DEBUG("\n");
+		}
+
+		for (int8_t i = 2 * _forder - 2; i >= 0; i--) {
+			if (i < _forder) {
+				z = 0.0f;
+
+			} else {
+				z = i - _forder + 1;
+			}
+
+			for (int8_t j = i - z; j >= z; j--) {
+				uint8_t row = j;
+				uint8_t col = i - j;
+				VTV(row, col) += (double)temp;
+			}
+
+			temp *= x;
+		}
+	}
+};

src/modules/tempcal/tempcal_main.cpp

@@ -65,7 +65,7 @@
 #include <controllib/uorb/blocks.hpp>
 
 #include <uORB/topics/sensor_gyro.h>
-#include "polyfit.h"
+#include "polyfit.hpp"
 
 #define DEBUG 0
 #if DEBUG
@@ -149,7 +149,7 @@ void Tempcal::task_main()
 	// subscribe to relevant topics
 	int gyro_sub[SENSOR_COUNT_MAX];
 	float gyro_sample_filt[SENSOR_COUNT_MAX][4];
-	polyfitter P[SENSOR_COUNT_MAX][3];
+	polyfitter<4> P[SENSOR_COUNT_MAX][3];
 	px4_pollfd_struct_t fds[SENSOR_COUNT_MAX] = {};
 	uint8_t _hot_soak_sat[SENSOR_COUNT_MAX] = {};
 	unsigned num_gyro = orb_group_count(ORB_ID(sensor_gyro));
@@ -171,9 +171,6 @@ void Tempcal::task_main()
 	for (uint8_t i = 0; i < num_gyro; i++) {
 		fds[i].fd = gyro_sub[i];
 		fds[i].events = POLLIN;
-		P[i][0].init(3);
-		P[i][1].init(3);
-		P[i][2].init(3);
 	}
 
 	// initialize data structures outside of loop
@@ -210,8 +207,8 @@ void Tempcal::task_main()
 
 				if (!_cold_soaked[i]) {
 					_cold_soaked[i] = true;
-					_low_temp[i] = gyro_data.temperature;	//Record the low temperature
+					_low_temp[i] = gyro_sample_filt[i][3];	//Record the low temperature
-					_ref_temp[i] = gyro_data.temperature + 12.0f;
+					_ref_temp[i] = gyro_sample_filt[i][3] + 12.0f;
 				}
 
 				num_samples[i]++;
@@ -235,6 +232,7 @@ void Tempcal::task_main()
 			} else {
 				continue;
 			}
+
 			//TODO: Hot Soak Saturation
 			if (_hot_soak_sat[i] == 10 || (_high_temp[i] - _low_temp[i]) > 24.0f) {
 				_hot_soaked[i] = true;