# How to Improve the Kalman Filter for Tracking the Periodic Motion of a Car?

I have a quite typical Kalman filter to design. I really read a lot of articles about the design of this filter but the performances of my filter are still quite bad.

Here is my situation. I have a small car that does periodic constant motion. This little car is moving uniformly in a straight line with v in time t, with a velocity of 0 in time 4t (You can think of it as the little car moving at a constant speed v to a target place and do something, and then moving at a constant speed v to the next location.) I can measure the speed v of the car (with noise) and I use the model as follows to estimate my little car:

X_ = X_last

P_ = P_last +Q

Here is my Matlab code: (I don't want to use the Matlab Kalman function ;) )


clear
clc
t = 1:628;
Z = square(t/50,50);
Z = Z+1.4;
Z = Z*80;
ZZ = randn(1,628);
ZZ = ZZ * 10;
Z = Z+ZZ;
plot(Z);

data = zeros(1,628);

Q = 0.1;
R = 3;
X_last = 0;
P_last = 1;
Dx = 0;

for i=1:628
X_ = X_last;
P_ = P_last +Q;
Kg = P_/(P_+R);
X = X_+Kg*(Z(i)-X_);
P = (1-Kg)*P_;

Dx = X_last;
P_last = P;
X_last = X;

data(i) = X;
end
plot(t,data,t,Z);


My question is, is there a more appropriate model of a Kalman filter for the type of car that I'm trying to predict? Although the model works well for constant velocity, there's a trailing when the velocity goes from V to zero as Fig. shown. Is there a good solution to that? Thanks!

PS: I control the velocity of the car by the current of its electrical motor, so I used the motor current to do matlab calculations（ y axis） • Do you know that the speed is either 0 or a constant V? Can you robustly detect the transition at time «t0»? If so, perhaps a plain average from t0 to t, reset each time there is some transition? – Knut Inge Jul 2 at 16:02
• Sorry，there's actually a very short transition time, not an instantaneous transition from 0 to V. It should look something like the blue line. What I'm trying to figure out, in this case, how should my model be adjusted to better fit this periodic signal? When V goes from zero to V it doesn't look like the right to use the constant velocity model. But the model of uniform motion works well for the rest of the time. How to deal with this problem? – Marcus Jul 3 at 0:56
• @Marcus, Do you have something missing in my answer I should add? – Royi Aug 4 at 16:03

You basically have 4 models here:

1. Accelerating to constant speed.
2. Moving at constant speed.
3. Decelerating to zero speed.
4. Standing.

So the basic solution is building the 4 models and switching using Hard Switch between them.
Yet there is a smoother framework to handle smooth transition between them called Interacting Multiple Model (IMM) Kalman Filter.

Using the IMM Framework (Which is basically adding weighing step for the models) with the knowledge of the Input as described in other answers will give you a much better results.

• Is there a good resource to read about Interacting Multiple Model (IMM) Kalman Filter? – David Aug 4 at 15:43
• I like Yaacov Bar Shalom - Estimation with Applications to Tracking and Navigation: Theory Algorithms and Software. – Royi Aug 4 at 16:01
• Thank you. I will have a look. – David Aug 11 at 12:12

My question is, is there a more appropriate model of a Kalman filter for the type of car that I'm trying to predict?

No. (But also please see below).

Although the model works well for constant velocity, there's a trailing when the velocity goes from V to zero as Fig. shown. Is there a good solution to that?

Yes.

The Kalman filter includes a term for "controls" or any sort of "input" which, if not provided, will be approximated (This is usually represented by the $$u$$ vector but here with $$B$$).

The throttle is a known variable and it drives the movement of your vehicle. In this particular case here, when the throttle is on, this places an expectation on the velocity of the vehicle (of course, the throttle can be at intermediate settings, not just on-off). When the throttle is off, this again implies that the velocity as derived by the "ideal model" (that the Kalman filter uses for the prediction phase) will trail off in a specific way. Bot the start and stop predicted transients of course here are not going to be instantaneous due to inertia (and will track your blue line more closely).

So, ideally the vehicle control inputs would have to be taken into acount for the output of the filter to "track" what is hapenning in reality more accurately.

Hope this helps.

• Hello！Thanks you very much for your answer. It's very useful. I adjusted the velocity curve I used to simulate, adding the transition that occurs during the start and stop process of the car, and the Kalman filter in this model worked well. However, I have a question, in the actual process, the speed will change, using this model X_ = X_last P_ = P_last +Q is really good to deal with the data (in simulate), but I think the model does not show the process of the speed would change. Excuse me, does this matter much? – Marcus Jul 3 at 11:46
• @Marcus Hello, glad to hear you found this helpful. I am afraid that I do not exactly get the clarifying question. In the "real" system, the Kalman filter needs to be "wired" to the throttle. Is this what you are asking? – A_A Jul 3 at 14:10
• Hello! Yes, that's pretty much the same question I'm wondering. The model I'm using now assumes that x_new=x_last (uniform velocity model), while in reality the velocity will have two uniform velocity states, V and 0. Is it appropriate to use a uniform velocity model for state switching ( the Kalman filter may not be "wired" to the throttle well at this time )? – Marcus Jul 3 at 14:30

My question is, is there a more appropriate model of a Kalman filter for the type of car that I'm trying to predict?

Probably yes, because you're generating a command for the car that (I presume) you know, but you're not using that knowledge in the filter.

The model you're using in the Kalman filter is $$\mathbf{x}_k = \mathbf{x}_{k-1} + \mathbf{w_k},\ \mathbf{y}_k = C \mathbf{x}_k$$ where $$\mathbf{x} = \begin{bmatrix}v\end{bmatrix}$$, $$v$$ is velocity, and C is, trivially, $$C = \begin{bmatrix}1\end{bmatrix}$$; i.e., today's velocity is yesterday's velocity plus some unknown noise, and you directly read velocity.

But you're leaving out the fact that the drive to your car is known*. You're also implicitly modeling the car as instantaneously responding to the drive command. The model for that would change $$\mathbf{x}$$ to $$\mathbf{x} = \begin{bmatrix}v_e\end{bmatrix}$$ and would add a term to the measurement such that $$\mathbf{y}_k = C \mathbf{x}_k + D\mathbf{u}$$, with $$\mathbf{u}$$ equal to your drive.

Then the line in your code that currently reads X = X_+Kg*(Z(i)-X_); would read X = X_+Kg*(Z(i) - drive(i) - X_);, where you've saved the 'perfect' value of Z as drive.

This will work great for modeling, at least if you don't care about tracking the transients of the car well as it accelerates and decelerates (which you're not modeling, so I'm not treating). In the real world, you'll find that the feedforward gain $$D$$ is not easy to pin down (and, in fact, won't be constant) -- but using it will increase the accuracy of your filter even in the real world.

* And you're apparently neglecting the fact that driving a motor with a current generates a more-or-less constant torque, not a more-or-less constant speed.