How to actually fuse sensor using Extended Kalman Filter

Question

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

1. E. I. Al Khatib, M. A. Jaradat, M. Abdel-Hafez and M. Roigari, "Multiple sensor fusion for mobile robot localization and navigation using the Extended Kalman Filter," 2015 10th International Symposium on Mechatronics and its Applications (ISMA), 2015, pp. 1-5, doi: 10.1109/ISMA.2015.7373480.
2. V. Sangale and A. Shendre, "Localization of a Mobile Autonomous Robot Using Extended Kalman Filter," 2013 Third International Conference on Advances in Computing and Communications, 2013, pp. 274-277, doi: 10.1109/ICACC.2013.59.
3. Jingang Yi, Junjie Zhang, Dezhen Song and Suhada Jayasuriya, "IMU-based localization and slip estimation for skid-steered mobile robots," 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2007, pp. 2845-2850, doi: 10.1109/IROS.2007.4399477.
4. C. Röhrig, D. Heß and F. Künemund, "Motion controller design for a mecanum wheeled mobile manipulator," 2017 IEEE Conference on Control Technology and Applications (CCTA), 2017, pp. 444-449, doi: 10.1109/CCTA.2017.8062502.

Answer 1

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

\begin{equation} \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] \end{equation}

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