I have read a lot of literature on this topic, but many questions still remain.
(Many formulae are here below, so, who prefers to look at them in a more comfortable manner can find a MathCAD sheet link in the end of the post).
So, we have a truck with an accelerometer and gyroscope. The following filter was obtained:
status vector X(6x1):
|x| - coordinate on the axis Х |y| - coordinate on the axis Y |φ| - robot turning angle |V| - robot linear velocity |ω| - robot angular velocity |a| - robot tangent acceleration
observation matrix Y(2x1):
|ya| - data from accelerometer |yg| - data from gyroscope
the system model (t — time step):
x = x + cos(φ) * (V * t + a * t^2/2) y = y + sin(φ) * (V * t + a * t^2/2) φ = φ + ω * t V = V + a * t ω = ω a = a
matrix F(6x6):
|1 0 -sin(φ)*(a*t^2 / 2+V*t) t*cos(φ) 0 t^2 *cos(φ)/2| |0 1 cos(φ)*(a*t^2 / 2+V*t) t*cos(φ) 0 t^2 *sin(φ)/2| |0 0 1 0 t 0 | |0 0 0 1 0 t | |0 0 0 0 t 0 | |0 0 0 0 0 t |
matrix H(2x6):
|0 0 0 0 0 1| |0 0 0 0 1 0|
matrix I(6x6) — identity matrix N(2x2) — identity matrix V(2x2) — identity matrix R(2x2) — identity matrix W(2x2) — identity
matrix Q(6x6): — I composed it empirically, tried to compute using a random sample, its variance for the diagonal items and covariance for all the rests
matrix P(6x6) — equals Q at the beginning
[xT] — transposed matrix, (x[-1]) — inverse matrix
the filter:
K = P*[HT] ((HP[HT] + VR[VT])[-1]) P=(I — KH)P x = x + K(y — h(x)) P=FP[FT] + WQ[WT] x = f(x,y)
I obtained diagrams for acceleration and turning angle. The acceleration diagram looks like it is to be, but the turning angle one is absolutely wrong. It was noticed, the matrix Q influences the filter strongly.
So, I have two questions: 1) did I compose the filter in right manner (if I had mistaken, please, let me know where); 2) how the matrix Q should be computed? (I even tried such a way: selected it empirically, and it worked very well, but I changed the system behaviour - acceleration - and the filter became incorrect).
All the calculations are in the MathCAD sheet.
