How to form Kalman filtering matrices for a problem with variable acceleration?

Question

Assuming we have

• time vector $T$ with constant time step $dt$
• position vector $X$
• velocity vector $V$
• acceleration vector $A$

All vectors $X, V, A$ have noise on their measurement ( $n_x$ , $n_v$ , $n_a$ ). And all should be filtered.

How to form Kalman filter for the the matrices:

$$ x_k= A x_{k-1} + B u_k + w_{k-1} $$ $$ y_k= H x_k + v_k $$

where $w$ and $n$ are normal noise. Each $x_k$ is $\begin{bmatrix} X_k & V_k & A_k\end{bmatrix}^T$

I came up with two matrix assumption for kalman filtering:

which one is correct? $$A=\begin{bmatrix} 1 & dt & \frac{1}{2}dt^2 \\ 0 & 1 & dt \\ 0 & 0 & 1 \end{bmatrix}$$ $$B=\begin{bmatrix}0 & 0 & 0\end{bmatrix}^T$$ $$H=I$$

or this one?

$$A=\begin{bmatrix} 1 & dt & \frac{1}{2}dt^2 \\ 0 & 1 & dt \\ 0 & 0 & 0 \end{bmatrix}$$ $$B=\begin{bmatrix} 0 & 0 & 1 \end{bmatrix}^T$$ $$H=I$$

The acceleration is not constant and there is a variable input acceleration. I need to fix acceleration as well as velocity and position.

Please explain why which one is correct and the other is wrong.

This is my code. How to extend it for variable acceleration?

time_resolution=0.016;
tmp1=diag((1:9)*0+1,3);
tmp2=diag((1:9)*0+1,6);
A_T=eye(9)+tmp1(1:9,1:9)*time_resolution+0.5*tmp2(1:9,1:9)*time_resolution*time_resolution;
A=A_T;
C=eye(size(A));
p_tmp=[position_x;position_y;position_z];
v_tmp=[velocity_x;velocity_y;velocity_z];
a_tmp=[acceleration_x;acceleration_y;acceleration_z];
X=[p_tmp;v_tmp;a_tmp];

xh_=X*0;
yh_=X*0;
inov=X*0;
xh=zeros(size(X)+[0,1]);
xh(:,1)=X(:,1);

R=1;
Q=100*diag([.1 .1 .1 .5 .5 .5 10 10 10]);
Px=eye(size(A));

for i = 1 : size(X,2)
    % x(t|t-1) = A*x(t-1|t-1)
    xh_(:,i) = A * xh(:,i);

    % P(t|t-1) = A*P(t-1|t-1)*A' + Q
    Px_ = A*Px*A' + Q;

    % K(t) = P(t|t-1) * C' * inv(C*P(t|t-1) * C' + R) )
    K = Px_ * C' /(C*Px_*C' + R);

    % y(t|t-1) = C*x(t|t-1) + R
    yh_(:,i) = C * xh_(:,i) + R;

    % innovation error  y(t) - y(t|t-1)
    inov(:,i) = X(:,i) - yh_(:,i);

    % x(t|t) = x(t|t-1) + K(t)*(y(t)-y(t|t-1))
    xh(:,i+1) = xh_(:,i) + K * inov(:,i);

    % P(t|t) = (I - K(t)*C) * P(t|t-1)
    Px = Px_ - K*C*Px_;
end

Answer 1

Take the first model and train your model noise matrix $\bf{Q}$ to account for the variable acceleration. A popular model is the Singer model from 1990 (see [1]).

[1] Singer, R. A., “Estimating Optimal Tracking Filter Performance for Manned Maneuvering Targets,” IEEE Transactions on Aerospace and Electronic Systems, vol. AES-6, no. 4, .pp. 473-483, July 1990.

Answer 2

Assuming $u_k$ is your input and you control the acceleration directly you should use the second one. If you use the first one your input will have no effect at all since $B u_k$ becomes exactly zero always. If you control the variations of the acceleration instead of the acceleration directly you should replace only your $A$ matrix by the first one.

Also, develop your matrix equations into scalar ones for each variable (position, speed, acceleration) and you won't need any help with this kind of question anymore