I have been developing a system using a moving robot with a distance sensor against another robot. I want to control these robots by estimating relative velocity and acceleration derived from the distance sensor. So I'd like to estimate relative velocity and acceleration using the measured value by the distance sensor. (Due to some restrictions, no additional sensor is allowed.)

Problem statement

I formulated the problem with a cubic Kalman filter configured as follows [1]: $$ \begin{align} \left[ \begin{array}{c} x_k \\ \dot{x}_k \\ \ddot{x}_k \end{array} \right] = \left[ \begin{array}{ccc} 1 && dt && dt^2/2 \\ 0 && 1 && dt \\ 0 && 0 && 1 \end{array} \right] \left[ \begin{array}{c} x_{k-1} \\ \dot{x}_{k-1} \\ \ddot{x}_{k-1} \end{array} \right] + \left[ \begin{array}{c} dt^3/6 \\ dt^2/2 \\ dt \end{array} \right] w_k, \end{align} $$ $$\begin{equation} z_k = x_k + v_k, \end{equation} $$ where process noise and observation noise follow gaussian distributions $w_k\sim\mathcal{N}(0,Q), v_k\sim\mathcal{N}(0,R)$.

I modeled $Q$ and $R$ as below. $$ Q=\begin{align} q\left[ \begin{array}{ccc} dt^5/20 && dt^4/8 && dt^3/6 \\ dt^4/8 && dt^3/3 && dt^2/2 \\ dt^3/6 && dt^2/2 && dt \end{array} \right] \end{align}, R=\frac{e_r^2/4+2r}{3} $$

Filtered signals have reasonable shapes as illustrated in the figure below but there exists large phase lag caused by the filter. It is not acceptable for realtime control of the robots in reaction to another robot.

I’m seeking a way to reduce the lag caused by the filter.

What I have tried

  • Tuning parameters of the filter
    Even with best parameters, I observed much phase lag.
  • Quadraric Kalman filter applied successively to get acceleration (first Kalman filter is applied to get velocity and another Kalman filter is applied to the estimated velocity to get acceleration.)
    Not much difference observed.
  • Kalman smoother as a fixed-lag smoother
    Not much difference observed.


I simulated the phase lag induced by the Kalman filter for illustration. I generated signals for simluation as follows:

Hz = 1000; % signal's frequency
time = 0:1/Hz:30; % time interval for simulation
accel = 200-400*cos(time*2*pi/40) - 500*sin(time/40) + 80*sin(time/10) .* cos(time/2-10) - 240*cos(time); % true acceleration
vel = cumtrapz(accel)/Hz; % velocity
dist = cumtrapz(vel)/Hz + 10*randn(1,numel(time)); % true distance with gaussian disturbance

Parameters used in the filter: $q = 20,~ er=1,~ r=10$. Blue lines show true distance/velocity/acceleration and red lines show filtered distance/velocity/acceleration. In acceleration estimation, we can observe large phase lag.

enter image description here

Any help would be warmly welcomed and appreciated.

EDIT@3/23/2020, UPDATE@3/25/2020

In response to a nice answer by @Luezoid, I also tried a constant jerk model instead of constant acceleration model [2]. The state transition matrix $F$ and covariance matrix of the process noise $Q$ is changed as follows: \begin{align} F&= \left[ \begin{array}{cccc} 1 && dt && dt^2/2 && dt^3/6 \\ 0 && 1 && dt && dt^2/2 \\ 0 && 0 && 1 && dt \\ 0 && 0 && 0 && 1 \\ \end{array} \right] ,\\ Q&=\left[ \begin{array}{cccc} dt^7/252 && dt^6/72 && dt^5/30 && dt^4/24 \\ dt^6/72 && dt^5/20 && dt^4/8 && dt^3/6 \\ dt^5/30 && dt^4/8 && dt^3/3 && dt^2/2 \\ dt^4/24 && dt^3/6 && dt^2/2 && dt \\ \end{array} \right], \end{align} where we assume time interval is sufficiently small. The other things remain the same as the constant acceleration model.

Here is the code.

x = dist; % dist: simulated signal above

dt = 1/Hz;

F = [1 dt dt^2/2 dt^3/6; 0 1 dt dt^2/2; 0 0 1 dt; 0 0 0 1]; % State trans matrix
H = [1 0 0 0]; % Observation matrix
Q = q * [dt^7/252 dt^6/72 dt^5/30 dt^4/24;...
         dt^6/72  dt^5/20 dt^4/8  dt^3/6;...
         dt^5/30  dt^4/8  dt^3/3  dt^2/2;...
         dt^4/24  dt^3/6  dt^2/2  dt]; % Covariance of process noise
R = (er^2/4+2*r)/3; % Covariance of measurement noise

x_est = [x(1);0;0;0]; % Filtered signal

P = eye(4,4); % Assuming initial estimate is correct

x_filtered = zeros(4,numel(x));
x_filtered(:,1) = x_est;

for t = 2:numel(x)
    z = x(t);

    x_est = F*x_est;  % Predicted State Estimate
    P = F*P*F' + Q;     % Predicted Error Covariance

    y = z - H*x_est;     % Innovation or Measurement Pre-fit Residual
    S = R + H*P*H';     % Innovation or Pre-fit Residual Covariance

    K = P*H'/S;         % Optimal Kalman Gain

    x_est = x_est + K*y;      % Updated State Estimate
    P = (eye(4,4) - K*H)*P;   % Updated Estimate Covariance

    x_filtered(:,t) = x_est;

I ran the simulation with the same data and obtained the results with different filter parameters.

Result 1 Constant jerk model with parameters: $q = 20,~ er=1,~ r=10$. enter image description here Result 2 Constant jerk model with parameters: $q = 50,~ er=1,~ r=10$. enter image description here

As observed in the results, I got larger amount of estimation error in acceleration compared to the constant acceleration model. Can we somehow reduce phase lag while keeping error level low?

[1]: L.J.Puglisi et al., On the velocity and Acceleration Estimation from Discrete Time-Position Sensors, CEAI, 2015.

[2]: K. Mehrotra and P. R. Mahapatra, A jerk model for tracking highly maneuvering targets, IEEE Transactions on Aerospace and Electronic Systems, vol. 33, no. 4, pp. 1094-1105, Oct. 1997.

  • $\begingroup$ Hi! You have about one second lag in acceleration estimation. Is it this lag you complain about ? Yet your position estimation seems quite accurate ? (may be still not enough for your application accuracy ?) $\endgroup$ – Fat32 Mar 9 at 22:36
  • $\begingroup$ @Fat32 Thanks for your comment. Yes the lag in acceleration is too large to build a control system that takes acceleration into account. $\endgroup$ – mhirano Mar 10 at 1:16
  • $\begingroup$ what happens if you increase sampling rate ? (decrease sampling period ?) $\endgroup$ – Fat32 Mar 10 at 2:04
  • $\begingroup$ @Fat32 I ran a simulation with a higher sampling rate and found it reduced the lag, but unfortunately the sampling rate of the distance sensor has already been set to its maximum. $\endgroup$ – mhirano Mar 10 at 2:36
  • $\begingroup$ Could you share more of the code? Maybe we'll spot something before writing an answer. $\endgroup$ – Royi Mar 23 at 12:46

Have you considered trying a constant jerk model as opposed to a constant acceleration model? Perhaps a higher order model would capture the acceleration better. See, for instance:

K. Mehrotra and P. R. Mahapatra, "A jerk model for tracking highly maneuvering targets," in IEEE Transactions on Aerospace and Electronic Systems, vol. 33, no. 4, pp. 1094-1105, Oct. 1997.

| improve this answer | |
  • $\begingroup$ Thanks for your answer! I tried the constant jerk model (4-state Kalman filter) and obtained less phase-lag acceleration estimation, but also introduced much larger error. I edit the question to share the trial. Can we somehow reduce phase lag while maintaining error level? $\endgroup$ – mhirano Mar 23 at 8:25
  • $\begingroup$ Just some thoughts... The simulated acceleration is periodic, so really you would need an infinite order polynomial to truly capture the process dynamics. The process noise captures this model mismatch, so increasing Q essentially gives the model more flexibility, but will increase the estimate noise. Decreasing Q increases lag. An excellent discussion on this topic which directly addresses tradeoffs in model order, selection of Q, and the resulting filter lag and performance can be found in Ch. 5 of Fundamentals of Kalman Filtering by Paul Zarchan. Highly recommend. $\endgroup$ – Luezoid Mar 23 at 15:34

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.