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）