polyfit.hpp

Go to the documentation of this file.

/****************************************************************************
 *
 *   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
*/

#pragma once
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/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
#define PF_DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__);
#else
#define PF_DEBUG(fmt, ...)
#endif

template<int _forder>
class polyfitter
{
public:
    polyfitter() {}

    void update(double x, double y)
    {
        update_VTV(x);
        update_VTY(x, y);
    }

    bool fit(double res[])
    {
        //Do inverse of VTV
        matrix::SquareMatrix<double, _forder> IVTV;

        IVTV = _VTV.I();

        for (int i = 0; i < _forder; i++) {
            for (int j = 0; j < _forder; j++) {
                PF_DEBUG("%.10f ", (double)IVTV(i, j));
            }

            PF_DEBUG("\n");
        }

        for (int i = 0; i < _forder; i++) {
            res[i] = 0.0;

            for (int j = 0; j < _forder; j++) {
                res[i] += IVTV(i, j) * (double)_VTY(j);
            }

            PF_DEBUG("%.10f ", res[i]);
        }

        return true;
    }

private:
    matrix::SquareMatrix<double, _forder> _VTV;
    matrix::Vector<double, _forder> _VTY;

    void update_VTY(double x, double y)
    {
        double temp = 1.0;
        PF_DEBUG("O %.6f\n", (double)x);

        for (int 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.0;
        int8_t z;

        for (int i = 0; i < _forder; i++) {
            for (int j = 0; j < _forder; j++) {
                PF_DEBUG("%.10f ", (double)_VTV(i, j));
            }

            PF_DEBUG("\n");
        }

        for (int i = 2 * _forder - 2; i >= 0; i--) {
            if (i < _forder) {
                z = 0.0f;

            } else {
                z = i - _forder + 1;
            }

            for (int j = i - z; j >= z; j--) {
                int row = j;
                int col = i - j;
                _VTV(row, col) += (double)temp;
            }

            temp *= x;
        }
    }
};