How to form Kalman filtering matrices for a problem with variable acceleration? 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  &amp; V_k &amp; A_k\end{bmatrix}^T$ 

I came up with two matrix assumption for kalman filtering:

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

or this one?

$$A=\begin{bmatrix} 1 &amp; dt &amp; \frac{1}{2}dt^2 \\ 0 &amp; 1 &amp; dt \\ 0 &amp; 0 &amp; 0 \end{bmatrix}$$
$$B=\begin{bmatrix} 0 &amp;  0 &amp; 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
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.