kalman.h

#ifndef KALMAN_H_LOADED
#define KALMAN_H_LOADED 1

#include "types.h"

class Kalman {
    // This implementation surreptitiously manages _two_ Kalman filters,
    // one for movement in the positive direction, and another for
    // movement in the negative direction (since the elevation drive moves
    // at different speeds going and and going down).  We update
    // either vmax_pos_ or vmax_neg_ in the two cases.
    // This should be transparent to the caller.

private:
    double theta_;
    double vmax_pos_;
    double vmax_neg_;
    double dtheta2_;
    double dvmax2_;
    double dz2_;

    // Internal covariance matrix
    double p00_, p01_, p10_, p11_;

    ms_t t_;                    // last update time

    char logchar_;              // if not '\0', then be chatty

public:
    Kalman();
    Kalman(double theta,        // initial position
           double maxthetadot,  // initial estimate of actual speed (rad/s) for commanded speed 255
           double dz,
           ms_t initial_time);
    // in both of the following, speed is given as a fraction of full speed
    void update(ms_t t, double speed);
    void click(ms_t t, double speed, double click_angle);

    void reset_angle(double ang);

    double angle(void) const { return theta_; }
    double fullspeed_pos(void) const { return vmax_pos_*1000.0; }
    double fullspeed_neg(void) const { return vmax_neg_*1000.0; }
    // are the following the best returns, here?
    double angle_error(void) const { return p00_; }
    double fullspeed_error(void) const { return p11_; }

    void verbose(char verbose_char) { logchar_ = verbose_char; }
};


#endif /* KALMAN_H_LOADED */