How to actually fuse sensor using Extended Kalman Filter

Background

I'm working on 4-omniwheel mobile robot. It have encoder on each wheel and MPU 6050 IMU. The robot positioning suffer a great error because slip, so i try to increase the accuracy of positioning by fusing sensor value from encoder and IMU.

Question

I have look a lot of paper regarding sensor fusion using EKF [1][2][3].

With algorithm of EKF

$$\hat\beta^-_k=f(\hat\beta^+_{k-1},u_k)$$ $$P^-_k=F_kP_{k-1}F^T_k+Q_k$$ $$K_k=P^-_kH^T_k(H_kP^-_kH^T_k+R_k)^{-1}$$ $$\hat\beta^+_{k-1}=\hat\beta^-_k+K_k(z_k-h(\hat\beta^-_k))$$ $$P^+_k=P^-_k-K_kH_kP^-_k$$

state to predict

$$\left[\begin{matrix} x \\ y \\ \theta \\ \dot x\\ \dot y\\ \omega\\ \end{matrix}\right]$$

and measurement model of each sensor

Encoder[4] $$h_{encoder} = \left[\begin{matrix}v_1\\v_2\\v_3\\v_4\\\end{matrix}\right]$$ $$H_{encoder} = \frac{r}{4} \left[\begin{matrix} 0 & 0 & 0 & 1 & 1 & \frac{1}{a+b}\\ 0 & 0 & 0 & 1 & -1 & \frac{-1}{a+b}\\ 0 & 0 & 0 & 1 & 1 & \frac{-1}{a+b}\\ 0 & 0 & 0 & 1 & -1 & \frac{1}{a+b}\\ \end{matrix}\right]$$ $$z_k = H_{encoder}\beta_k+v_k$$

IMU $$h_{imu} = \left[\begin{matrix}\theta\\\omega\end{matrix}\right]$$ $$H_{imu} = \left[\begin{matrix} 0 & 0 & 1 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 1\end{matrix}\right]$$ $$z_k = H_{imu}\beta_k+v_k$$

I'm not quite sure where to put both sensor equation. I mean in EKF algorithm, I can literally place $$h, H$$ and $$z_k$$ of sensor on Step 3, 4 and 5 one at a time. But how to put both of IMU and Encoder equation? Or maybe I have misunderstanding about EKF sensor fusion?

Reference

• In a loop, you need to decide from where the measurements come from and modify these $K_k, \hat\beta^+_{k-1}, K_k(z_k-h(\hat\beta^-_k))$. If you have 4 measurements, then iterate over all of them. Commented Oct 9, 2021 at 9:35

I've never worked on Mecanum wheels before so I researched a bit. One of the first things I look for is there has to be a way to combine all of the encoder velocity measurements. Apparently, Jacobian equation can be used for that (I believe it is H_encoder in your question).

You can calculate system's velocity by

$$$$\left[\begin{array}{c} V_{X} \\ V_{Y} \\ \omega_{Z} \end{array}\right]=J^{+}\left[\begin{array}{c} V_{1 W} \\ V_{2 W} \\ V_{3 W} \\ V_{4 W} \end{array}\right]$$$$

where J+ is pseudoinverse matrix. Using the fact that lineer velocity is just a product of angular velocity and wheel radius,

$$\left[\begin{array}{c} V_{X} \\ V_{Y} \\ \omega_{Z} \end{array}\right]=\frac{R}{4}\left[\begin{array}{cccc} -1 & 1 & 1 & -1 \\ 1 & 1 & 1 & 1 \\ -\frac{1}{a} & \frac{1}{a} & -\frac{1}{a} & \frac{1}{a} \end{array}\right]\left[\begin{array}{c} \dot{\theta}_{1} \\ \dot{\theta}_{2} \\ \dot{\theta}_{3} \\ \dot{\theta}_{4} \end{array}\right]$$

From here, you can easily calculate position by integrating this equation. Using limit definition of derivatives you have,

$$\frac{\left[\begin{array}{c} X_{X} \\ Y_{Y} \\ \theta_{Z} \end{array}\right]_{k} - \left[\begin{array}{c} X_{X} \\ Y_{Y} \\ \theta_{Z} \end{array}\right]_{k-1}}{ \delta T}=\frac{R}{4}\left[\begin{array}{cccc} -1 & 1 & 1 & -1 \\ 1 & 1 & 1 & 1 \\ -\frac{1}{a} & \frac{1}{a} & -\frac{1}{a} & \frac{1}{a} \end{array}\right]\left[\begin{array}{c} \dot{\theta}_{1} \\ \dot{\theta}_{2} \\ \dot{\theta}_{3} \\ \dot{\theta}_{4} \end{array}\right]_{k}$$

Rewriting,

$$\left[\begin{array}{c} X_{X} \\ Y_{Y} \\ \theta_{Z} \end{array}\right]_{k}=\left[\begin{array}{c} X_{X} \\ Y_{Y} \\ \theta_{Z} \end{array}\right]_{k-1}+\frac{R}{4}\left[\begin{array}{cccc} -1 & 1 & 1 & -1 \\ 1 & 1 & 1 & 1 \\ -\frac{1}{a} & \frac{1}{a} & -\frac{1}{a} & \frac{1}{a} \end{array}\right]\left[\begin{array}{c} \dot{\theta}_{1} \\ \dot{\theta}_{2} \\ \dot{\theta}_{3} \\ \dot{\theta}_{4} \end{array}\right]_{k} \delta T$$

So with these equations, you have propagation equations of the system (just add Gaussian white noise for measurement equations). This brings us to a competitive sensor fusion on theta value, since both IMUs and encoders are "sensing" it. You can check on some competitive sensor fusion algorithms. Most of the time people just average them. After that, you will have simple H matrix for kalman filter.

$$H = \begin{bmatrix} 1 & 0 & 0 &0 & 0 &0 \\ 0 & 1 & 0 & 0 & 0 &0 \\ 0 &0 &1 & 0 & 0 &0 \\ 0& 0 & 0 & 0 & 0 & 1 \end{bmatrix}$$

Edit: Forgot to give reference: Kalman Filter Sensor Fusion for Mecanum Wheeled Automated Guided Vehicle Localization

• Thank you for your answer, i will take a look at your advice. Yes I put jacobian matrix of mobile robot in H matrix because it map each wheel velocity into cartesian velocity of mobile robot. Commented Jun 9, 2021 at 8:57