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, privacy policy and cookie policy

Browse other questions tagged or ask your own question.