5
$\begingroup$

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.

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

$\endgroup$
12
  • $\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 '20 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 '20 at 1:16
  • $\begingroup$ what happens if you increase sampling rate ? (decrease sampling period ?) $\endgroup$
    – Fat32
    Mar 10 '20 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 '20 at 2:36
  • $\begingroup$ Could you share more of the code? Maybe we'll spot something before writing an answer. $\endgroup$
    – Royi
    Mar 23 '20 at 12:46
4
$\begingroup$

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:

$\endgroup$
3
$\begingroup$

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.

$\endgroup$
2
  • $\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 '20 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 '20 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.