mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-17 13:27:35 +08:00
tempcal: move to using Matrix library functions
This commit is contained in:
Siddharth Bharat Purohit
committed by
Lorenz Meier
Lorenz Meier
parent
0cc034ee15
commit
86d9ba9cde
@@ -38,8 +38,6 @@ px4_add_module(
|
||||
STACK_MAX 4000
|
||||
SRCS
|
||||
tempcal_main.cpp
|
||||
polyfit.cpp
|
||||
matrix_alg.cpp
|
||||
DEPENDS
|
||||
platforms__common
|
||||
)
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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);
|
||||
@@ -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);
|
||||
};
|
||||
@@ -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;
|
||||
VTV = new double[_forder * _forder];
|
||||
public:
|
||||
polyfitter() {}
|
||||
|
||||
if (VTV == NULL) {
|
||||
return -1;
|
||||
void update(double x, double y)
|
||||
{
|
||||
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) {
|
||||
return -1;
|
||||
}
|
||||
IVTV = VTV.I();
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
};
|
||||
@@ -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
|
||||
_ref_temp[i] = gyro_data.temperature + 12.0f;
|
||||
_low_temp[i] = gyro_sample_filt[i][3]; //Record the low temperature
|
||||
_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;
|
||||
|
||||
Reference in New Issue
Block a user