0
$\begingroup$

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
$\endgroup$
1
$\begingroup$

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.

$\endgroup$
0
$\begingroup$

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

$\endgroup$
  • $\begingroup$ There is a car and the driver is accelerating. I don't have the driver input. I just have the measurements. None of position, velocity, and acceleration is accurate. I need to compromise between them. $\endgroup$ – Nofoos Jun 5 '15 at 1:41
  • $\begingroup$ If your model is purely observed (not controlled) there's no reason to put B or u_k in your equations $\endgroup$ – gsmafra Jun 5 '15 at 17:39

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.