Skip to content

Instantly share code, notes, and snippets.

@Lauszus

Lauszus/polyfit.py

Last active August 17, 2018 08:36
Show Gist options
  • Save Lauszus/5282a1121aca1a1ad45d09892f67abb4 to your computer and use it in GitHub Desktop.
Save Lauszus/5282a1121aca1a1ad45d09892f67abb4 to your computer and use it in GitHub Desktop.
Download ZIP
Modified PX4 polyfit class to use Eigen instead of PX4 Matrix library and added Python version
Raw
polyfit.py
#!/usr/bin/python
# /****************************************************************************
# *
# * 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.
# *
# ****************************************************************************/
#
#
# This algorithm performs a curve fit of m x,y data points using a polynomial
# equation of the following form:
#
# yi = a0 + a1.xi + a2.xi^2 + a3.xi^3 + .... + an.xi^n + ei , where:
#
# i = [0,m]
# xi is the x coordinate (independant variable) of the i'th measurement
# yi is the y coordinate (dependant variable) of the i'th measurement
# ei is a random fit error being the difference between the i'th y coordinate
# and the value predicted by the polynomial.
#
# In vector form this is represented as:
#
# Y = V.A + E , where:
#
# V is Vandermonde matrix in x -> https://en.wikipedia.org/wiki/Vandermonde_matrix
# Y is a vector of length m containing the y measurements
# E is a vector of length m containing the fit errors for each measurement
#
# Use an Ordinary Least Squares derivation to minimise ∑(i=0..m)ei^2 -> https://en.wikipedia.org/wiki/Ordinary_least_squares
#
# Note: In the wikipedia reference, the X matrix in reference is equivalent to our V matrix and the Beta matrix is equivalent to our A matrix
#
# A = inv(transpose(V)*V)*(transpose(V)*Y)
#
# We can accumulate VTV and VTY recursively as they are of fixed size, where:
#
# VTV = transpose(V)*V =
# __ __
# | m+1 x0+x1+...+xm x0^2+x1^2+...+xm^3 .......... x0^n+x1^n+...+xm^n |
# |x0+x1+...+xm x0^2+x1^2+...+xm^3 x0^3+x1^3+...+xm^3 .......... x0^(n+1)+x1^(n+1)+...+xm^(n+1) |
# | . . . . |
# | . . . . |
# | . . . . |
# |x0^n+x1^n+...+xm^n x0^(n+1)+x1^(n+1)+...+xm^(n+1) x0^(n+2)+x1^(n+2)+...+xm^(n+2) .... x0^(2n)+x1^(2n)+...+xm^(2n) |
# |__ __|
#
# and VTY = transpose(V)*Y =
# __ __
# | ∑(i=0..m)yi |
# | ∑(i=0..m)yi*xi |
# | . |
# | . |
# | . |
# |∑(i=0..m)yi*xi^n|
# |__ __|
#
# Polygon linear fit
# Author: Siddharth Bharat Purohit
# Python version by Kristian Sloth Lauszus
# Original: https://github.com/PX4/Firmware/blob/9ce83f22087311df06078edf3a9da640d93ab396/src/modules/events/temperature_calibration/polyfit.hpp
import numpy as np
import matplotlib.pyplot as plt
class Polyfit(object):
def __init__(self, coefs):
self.coefs = coefs
self._VTV = np.zeros(shape=(self.coefs, self.coefs)) # Square matrix
self._VTY = np.zeros(shape=(self.coefs, 1)) # Vector
def update(self, x: float, y: float) -> None:
self._update_VTV(x)
self._update_VTY(x, y)
def fit(self) -> np.ndarray:
return np.squeeze(np.linalg.inv(self._VTV).dot(self._VTY))
def _update_VTY(self, x: float, y: float) -> None:
xi_n = 1.0
for i in reversed(range(0, self.coefs)):
self._VTY[i] += y * xi_n
xi_n *= x
def _update_VTV(self, x: float) -> None:
xm_n = 1.0
for i in reversed(range(0, 2 * self.coefs - 1)):
if i < self.coefs:
z = 0
else:
z = i - self.coefs + 1
for j in reversed(range(z, i - z + 1)):
row = j
col = i - j
self._VTV[row, col] += xm_n
xm_n *= x
def main():
polyfit = Polyfit(3 + 1) # 3 order
temperatures = []
measurements = []
for i in range(0, 10):
temp = i
data = 3 * i + 5 * i*i + 7 * i*i*i
temperatures.append(temp)
measurements.append(data)
polyfit.update(temp, data)
plt.plot(temperatures, measurements, linestyle='--')
res = polyfit.fit()
res[-1] = 0 # Normalise the correction to be zero at the reference temperature
print('y = %f + %f*x + %f*x^2 + %f*x^3' % (res[3], res[2], res[1], res[0]))
x = np.arange(0, 10, 0.25)
y = res[3] + res[2] * x + res[1] * x ** 2 + res[0] * x ** 3
plt.plot(x, y)
plt.show()
if __name__ == '__main__':
main()
Raw
polyfit_eigen.hpp
/****************************************************************************
*
* 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.
*
****************************************************************************/
/*
This algorithm performs a curve fit of m x,y data points using a polynomial
equation of the following form:
yi = a0 + a1.xi + a2.xi^2 + a3.xi^3 + .... + an.xi^n + ei , where:
i = [0,m]
xi is the x coordinate (independant variable) of the i'th measurement
yi is the y coordinate (dependant variable) of the i'th measurement
ei is a random fit error being the difference between the i'th y coordinate
and the value predicted by the polynomial.
In vector form this is represented as:
Y = V.A + E , where:
V is Vandermonde matrix in x -> https://en.wikipedia.org/wiki/Vandermonde_matrix
Y is a vector of length m containing the y measurements
E is a vector of length m containing the fit errors for each measurement
Use an Ordinary Least Squares derivation to minimise ∑(i=0..m)ei^2 -> https://en.wikipedia.org/wiki/Ordinary_least_squares
Note: In the wikipedia reference, the X matrix in reference is equivalent to our V matrix and the Beta matrix is equivalent to our A matrix
A = inv(transpose(V)*V)*(transpose(V)*Y)
We can accumulate VTV and VTY recursively as they are of fixed size, where:
VTV = transpose(V)*V =
__ __
| m+1 x0+x1+...+xm x0^2+x1^2+...+xm^3 .......... x0^n+x1^n+...+xm^n |
|x0+x1+...+xm x0^2+x1^2+...+xm^3 x0^3+x1^3+...+xm^3 .......... x0^(n+1)+x1^(n+1)+...+xm^(n+1) |
| . . . . |
| . . . . |
| . . . . |
|x0^n+x1^n+...+xm^n x0^(n+1)+x1^(n+1)+...+xm^(n+1) x0^(n+2)+x1^(n+2)+...+xm^(n+2) .... x0^(2n)+x1^(2n)+...+xm^(2n) |
|__ __|
and VTY = transpose(V)*Y =
__ __
| ∑(i=0..m)yi |
| ∑(i=0..m)yi*xi |
| . |
| . |
| . |
|∑(i=0..m)yi*xi^n|
|__ __|
*/
/*
Polygon linear fit
Author: Siddharth Bharat Purohit
Modified by Kristian Sloth Lauszus to use Eigen instead of the PX4 Matrix library
Original: https://github.com/PX4/Firmware/blob/9ce83f22087311df06078edf3a9da640d93ab396/src/modules/events/temperature_calibration/polyfit.hpp
*/
#pragma once
#include <Eigen/Dense>
using namespace Eigen;
template<int coefs>
class PolyfitterEigen {
public:
PolyfitterEigen() {
VTV.setZero();
VTY.setZero();
}
void update(double x, double y) {
update_VTV(x);
update_VTY(x, y);
}
void fit(double res[coefs]) {
Matrix<double, coefs, 1> A = VTV.inverse() * VTY;
for (int i = 0; i < coefs; i++)
res[i] = A[i];
}
private:
Matrix<double, coefs, coefs> VTV;
Matrix<double, coefs, 1> VTY;
void update_VTY(double x, double y) {
double xi_n = 1.0;
for (int i = coefs - 1; i >= 0; i--) {
VTY(i) += y * xi_n;
xi_n *= x;
}
}
void update_VTV(double x) {
double xm_n = 1.0;
for (int i = 2 * coefs - 2; i >= 0; i--) {
int z = i < coefs ? 0 : i - coefs + 1;
for (int j = i - z; j >= z; j--) {
int row = j;
int col = i - j;
VTV(row, col) += xm_n;
}
xm_n *= x;
}
}
};
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment