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)