PX4-Autopilot/test/filter.cpp

#include <assert.h>
#include <stdio.h>

#include <matrix/filter.hpp>

using namespace matrix;

template int kalman_correct<float, 6, 5>(
    const SquareMatrix<float, 6> & P,
    const Matrix<float, 5, 6> & C,
    const SquareMatrix<float, 5> & R,
    const Vector<float, 5> &r,
    Vector<float, 6> & dx,
    SquareMatrix<float, 6> & dP,
    float & beta
);

int main()
{
    const size_t n_x = 6;
    const size_t n_y = 5;
    SquareMatrix<float, n_x> P = eye<float, n_x>();
    SquareMatrix<float, n_y> R = eye<float, n_y>();
    Matrix<float, n_y, n_x> C;
    C.setIdentity();
    float data[] = {1,2,3,4,5};
    Vector<float, n_y> r(data);

    Vector<float, n_x> dx;
    SquareMatrix<float, n_x> dP;
    float beta = 0;
    kalman_correct<float, 6, 5>(P, C, R, r, dx, dP, beta);

    dx.T().print();
    printf("beta: %g\n", beta);

    float data_check[] = {0.5,1,1.5,2,2.5,0};
    Vector<float, n_x> dx_check(data_check);
    assert(dx == dx_check);


    return 0;
}

/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */