Background: I'm building a 3 omniwheel rover platform that looks something like this: 3 omniwheels rover platform

It has 1 IMU sensors on each of the wheels (3 in total). So in theory, I can get gyroscope and accelerometer data from each of the wheels and use that information to calculate the current speed of the wheels.

So let's say this is my model:

$x_k = \begin{bmatrix} v_k\\a_k \end{bmatrix} = Ax_{k-1} + Bu_{k-1}$

where: $A = \begin{bmatrix} 0&\Delta t \\ 0&0 \end{bmatrix}$ and $B = \begin{bmatrix} 1&0\\0&0 \end{bmatrix}$ and $ u_{k-1} = \begin{bmatrix} u_{velocity}\\0 \end{bmatrix}$

I can "control" the speed of each wheels, but of course that's not accurate. But I cannot control the acceleration of each wheels, so I let $u_{acceleration}$ in matrix $u_{k-1}$ to be 0.

$z_k = \begin{bmatrix} v_{measure}\\a_{measure} \end{bmatrix} = H\begin{bmatrix} \omega_{measure}\\a_{measure} \end{bmatrix}$ with $H = \begin{bmatrix} R&0\\0&1 \end{bmatrix}$

$\omega_{measure}$ is the data from gyroscope and $a_{measure}$ is the data from accelerometer (of each wheels).


$x_k = Ax_{k-1} + Bu_{k-1}$

$P_k = AP_{k-1}A^T + Q$

I haven't really make up my mind about what Q should be.


$K_k = P_kH^t(HP_kH^T + R)^{-1}$

$x_k = x_k + K_k(z_k - Hx_k)$

$P_k = (I - K_kH)P_k$

I haven't made up my mind about choosing R.

I'm also aware that I dropped all the negative sign and the hat sign but hopefully it won't bother the readers too much.

What do you think about this module? Should I change something? Is something is not correct? This is my first time trying out Kalman filter (or any filter at all) after spending a full day learning about it.

In the future I would also like to add an encoder on each wheels to get a better reading.

  • $\begingroup$ Although I can tell each of the wheel to operate at a certain speed, there's nothing guarantee that it will operate at that speed. Should this be consider as a control input? Should I leave B = 0 and let A = [1 t][0 0] ? $\endgroup$
    – Anh Tran
    Jan 18, 2019 at 4:10
  • 1
    $\begingroup$ What is the objective? To keep the robot on a route? To regulate the speed? $\endgroup$
    – A_A
    Jan 18, 2019 at 10:08
  • $\begingroup$ @A_A Basically it's to get the current position of the robot while it is moving on a path. But I have another algorithm to calculate the position that is quite accurate, but it is based on the current speed of 3 wheels (without sensor feedback, using stepper motors give reasonable result but I'm working on making it more accurate). What do you think? $\endgroup$
    – Anh Tran
    Jan 18, 2019 at 11:39
  • $\begingroup$ So, the motors on the wheels are stepper motors? $\endgroup$
    – A_A
    Jan 18, 2019 at 13:48
  • $\begingroup$ @A_A yes, it is stepper motors. $\endgroup$
    – Anh Tran
    Jan 19, 2019 at 5:06


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Browse other questions tagged or ask your own question.