# How to Reduce Phase Lag Caused by Kalman Filter

Background

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 : \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.

Demonstration

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. 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 . 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;
end


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$$. Result 2 Constant jerk model with parameters: $$q = 50,~ er=1,~ r=10$$. 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?

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

: 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.

• 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 ?) – Fat32 Mar 9 at 22:36
• @Fat32 Thanks for your comment. Yes the lag in acceleration is too large to build a control system that takes acceleration into account. – mhirano Mar 10 at 1:16
• what happens if you increase sampling rate ? (decrease sampling period ?) – Fat32 Mar 10 at 2:04
• @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. – mhirano Mar 10 at 2:36
• Could you share more of the code? Maybe we'll spot something before writing an answer. – Royi Mar 23 at 12:46