root/applications/doprava/QuadraticMinimalizator.h

#ifndef QUADRATICMINIMALIZATOR_H
#define QUADRATICMINIMALIZATOR_H

#include <cstdlib>
#include <itpp/itbase.h>
#include <iostream>


using namespace itpp;
using namespace std;

class QuadraticMinimalizator {
protected:
    mat A;
    mat B;
    mat Q;
    mat R;
    double maxdif;

    bool test() {
        if ( !isDiagonal(Q) ) {
            stringstream e;
            e << "Matrix Q is not diagonal" << endl;
            e << Q << endl;
            it_warning(e.str());
                        return false;
        }
        if ( !isDiagonal(R) ) {
            stringstream e;
            e << "Matrix R is not diagonal" << endl;
            e << R << endl;
            it_warning(e.str());
                        return false;
        }
        if ( !isPositiveDefinite(Q) ) {
            stringstream e;
            e << "Matrix Q is not positive definite" << endl;
            e << Q << endl;
            it_warning(e.str());
                        return false;
        }
        if ( !isPositiveDefinite(R) ) {
            stringstream e;
            e << "Matrix R is not positive definite" << endl;
            e << R << endl;
            it_warning(e.str());
                        return false;
        }
                return true;
    }

public:
    QuadraticMinimalizator( const mat &A, const mat &B, const mat &Q, const mat &R ) {
        maxdif = 0.00000001;
        this->A = A;
        this->B = B;
        this->Q = Q;
        this->R = R;
        test();
    }

    /**
     * x(t+1) = Ax(t) + Bu(t)
     * @param horizont
     * @param C
     * @return L: u(t) = Lx(t)
     */
    mat L( const int horizont ) {
        int xdim = A.cols();
        int udim = B.cols();
        mat sqQ = sqrt(Q);
        mat sqR = sqrt(R);
        mat M0, M;
        mat qrQ;
        mat Lall, Ls, Lx, Lu, L;
        M0 = concat_vertical(
                concat_horizontal( sqQ * B,     sqQ * A ),
                concat_horizontal( sqR,         zeros( udim, xdim ) )
            );
        M = M0;
        for (int h = horizont; h >= 0; h --) {
            qr(M, Lall);
            Lu = Lall(0,    udim-1,       0,    udim-1);
            Lx = Lall(udim, udim+xdim-1,  udim, udim+xdim-1);
            Ls = Lall(0,    udim-1,       udim, udim+xdim-1);
            M = concat_vertical(
                    M0,
                    concat_horizontal(Lx * B, Lx * A )
                );
        }
        L = - inv(Lu) * Ls;
        return L;
    }

        /*
    double maxDif( const mat & M1, const mat & M2 ) {
        return max(max(abs(M1 - M2), 1));
    }

    bool isSymmetric( const mat & M) {
        return isSymmetric(M, maxdif);
    }

    bool isSymmetric( const mat & M, double maxdif ) {
        mat D = abs(M - M.transpose());
        double m = max(max(D, 1));
        if ( m < maxdif )
            return true;
        else
            return false;
    }
*/
    bool isDiagonal(  const mat M ) {
        mat D = abs(M);
        for ( int i = 0; i < M.rows(); i ++ ) {
            for ( int j = 0; j < M.cols(); j ++ ) {
                if ( i != j && D(i,j) > maxdif ) {
                    return false;
                }
            }
        }
        return true;
    }
        

    bool isPositiveDefinite( const mat M ) {
        int dim = M.rows();
        if ( dim != M.cols() ) {
            stringstream err;
            err << "Matrix " << M << " is not square";
            it_warning(err.str());
        } else {
            for ( int i = dim-1; i >= 0; i-- ) {
                // subdeterminanty
                if ( det(M(0,i,0,i)) <= 0 ) {
                    return false;
                }
            }
            return true;
        }
        return false;
    }




};

#endif  /* QUADRATICMINIMALIZATOR_H */