How to Reduce Phase Lag Caused by Kalman Filter

Question

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

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 [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;
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?

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

Answer 1

What you're experiencing is the transient lag of the Kalman Filter.
The Kalman Filter, using the Measurement and Process Noise balances between begin very adaptive to being an aggressive smoother.

In your case it means either having short lag with high error in steady state or having long lag with small error in steady state.
In other words, either have low error on transients yet larger error on steady state or higher error on transients and lower error on steady state.

One way to deal with such issue is using Interacting Multiple Model (IMM) Kalman Filters where you have multiple Kalman Models and you jump from one to another according to need.

Another, different approach, is using FIR Filtering.
Which doesn't have the infinite horizon (Memory) of the Kalman Filter.

You may read the work by Yuriy S. Shmaliy. Specifically the paper Unbiased FIR Filtering: An Iterative Alternative to Kalman Filtering Ignoring Noise and Initial Conditions (PDF version on ResearchGate).

With finite horizon of an FIR Filter (Basically derived by Least Squares) you may have higher RMSE in theory (Since the Kalman Filter is optimal for the case the model is the same for infinite samples) but in practice you get good results with better balance of the transients and sensitivity to initialization (Which is the other side of the transients sensitivity).

Resources on Optimal FIR:

• Optimal FIR Estimator for Discrete Time-variant State Space Model.
• Iterative Form for Optimal FIR Filtering of Time Variant Systems.
• An Approach to Optimal Filtering of Time Variant Systems via Finite Measurements.
• Fast Kalman Like Optimal Unbiased FIR Filtering with Applications.
• Ultimate Iterative UFIR Filtering Algorithm.
• Unified Maximum Likelihood Form for Bias Constrained FIR Filters.
• An Iterative Filter with Finite Measurements for Suddenly Maneuvering Targets.
• A Revisit to Strictly Passive FIR Filtering.
• Unbiased FIR Filtering: An Iterative Alternative to Kalman Filtering Ignoring Noise and Initial Conditions.
• Review of Unbiased FIR Filters, Smoothers, and Predictors for Polynomial Signals.
• Iterative Maximum Likelihood FIR Estimation of Dynamic Systems With Improved Robustness.
• Comparing Robustness of the Kalman, H∞, and UFIR Filters.
• Fast Kalman Like Optimal FIR Filter for Time Variant Systems with Improved Robustness.
• An Improved Iterative FIR State Estimator and Its Applications.
• Kalman and UFIR State Estimation with Colored Measurement Noise using Backward Euler Method.
• Algorithms Design for Tracking Moving Objects with Colored Process Noise.
• Denoising ECG Signals Using Unbiased FIR Smoother and Harmonic State Space Model.

Answer 2

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.