kalman.h

Go to the documentation of this file.

/*
 * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
 * Copyright (C) 2006-2010 RobotCub Consortium
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms
 * of the BSD-3-Clause license. See the accompanying LICENSE file for
 * details.
*/
 
#ifndef __KALMAN_H__
#define __KALMAN_H__
 
#include <yarp/sig/Vector.h>
#include <yarp/sig/Matrix.h>
#include <iCub/ctrl/math.h>
 
 
namespace iCub
{
 
namespace ctrl
{
 
class Kalman
{
protected:
    yarp::sig::Matrix A, At;
    yarp::sig::Matrix H, Ht;
    yarp::sig::Matrix B;
    yarp::sig::Matrix Q;
    yarp::sig::Matrix R;
    yarp::sig::Matrix I;
    
    yarp::sig::Vector x;
    yarp::sig::Matrix P;
    yarp::sig::Matrix K;
    yarp::sig::Matrix S;
    double validationGate;
 
    size_t n;
    size_t m;
 
    void initialize();
 
    // Default constructor: not implemented.
    Kalman();
 
public:
    Kalman(const yarp::sig::Matrix &_A, const yarp::sig::Matrix &_H,
           const yarp::sig::Matrix &_Q, const yarp::sig::Matrix &_R);
 
    Kalman(const yarp::sig::Matrix &_A, const yarp::sig::Matrix &_B,
           const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_Q,
           const yarp::sig::Matrix &_R);
 
    bool init(const yarp::sig::Vector &_x0, const yarp::sig::Matrix &_P0);
 
    const yarp::sig::Vector& predict(const yarp::sig::Vector &u);
 
    const yarp::sig::Vector& predict();
 
    const yarp::sig::Vector& correct(const yarp::sig::Vector &z);
 
    const yarp::sig::Vector& filt(const yarp::sig::Vector &u, const yarp::sig::Vector &z);
 
    const yarp::sig::Vector& filt(const yarp::sig::Vector &z);
 
    const yarp::sig::Vector& get_x() const { return x; }
 
    yarp::sig::Vector get_y() const;
 
    const yarp::sig::Matrix& get_P() const { return P; }
 
    const yarp::sig::Matrix& get_S() const { return S; }
 
    double get_ValidationGate() const { return validationGate; }
 
    const yarp::sig::Matrix& get_K() const { return K; }
 
    const yarp::sig::Matrix& get_A() const { return A; }
 
    const yarp::sig::Matrix& get_B() const { return B; }
 
    const yarp::sig::Matrix& get_H() const { return H; }
 
    const yarp::sig::Matrix& get_Q() const { return Q; }
 
    const yarp::sig::Matrix& get_R() const { return R; }
 
    bool set_A(const yarp::sig::Matrix &_A);
 
    bool set_B(const yarp::sig::Matrix &_B);
 
    bool set_H(const yarp::sig::Matrix &_H);
 
    bool set_Q(const yarp::sig::Matrix &_Q);
 
    bool set_R(const yarp::sig::Matrix &_R);
};
 
}
 
}
 
#endif