Kalman filter implementation question

189 Views Asked by At

I have the following code to define a Kalman filter:

    void makeBaseV() { 
        V(1, 1) = 1.0;
    }

    void makeBaseW() {
        W(1, 1) = 1.0;
        W(1, 2) = 0.0;
        W(1, 3) = 0.0;
        W(2, 1) = 0.0;
        W(2, 2) = 1.0;
        W(2, 3) = 0.0;
        W(3, 1) = 0.0;
        W(3, 2) = 0.0;
        W(3, 3) = 1.0;
    }

    void makeA() {
        double T = Period;
        A(1, 1) = 1.0;
        A(1, 2) = T;
        A(1, 3) = (T*T) / 2;
        A(2, 1) = 0.0;
        A(2, 2) = 1.0;
        A(3, 3) = T;
        A(3, 1) = 0.0;
        A(3, 2) = 0.0;
        A(3, 3) = 1.0;
    }
    void makeH() {
        H(1, 1) = 1.0;
        H(1, 2) = 0.0;
        H(1, 3) = 0.0;
    }
    void makeProcess() {
        double T = u(1);
        Vector x_(x.size());
        x_(1) = x(1) + x(2) * T + (x(3) * T*T / 2);
        x_(2) = x(2) + x(3) * T;
        x_(3) = x(3);
        x.swap(x_);
    }
    void makeMeasure() {
        z(1) = x(1);
    }

My state $\vec x_{k} = [p, v, a]$ where $p$ is position, $v$ is velocity, and $a$ is acceleration. My measurement vector $\vec z_{k}$ is the position. Is my math for matrices $V$, $W$, $H$, and $A$ correct for my system? Also what should $Q$ and $R$ looks like?

I only ask because I'm currently getting undefined results.(1.0/0.0)