# Is my Kalman Filter model reasonable? Using for a 3 wheels rover platform

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

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).

Predict:

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

Update:

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

• 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] ? Jan 18, 2019 at 4:10
• What is the objective? To keep the robot on a route? To regulate the speed?
– A_A
Jan 18, 2019 at 10:08
• @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? Jan 18, 2019 at 11:39
• So, the motors on the wheels are stepper motors?
– A_A
Jan 18, 2019 at 13:48
• @A_A yes, it is stepper motors. Jan 19, 2019 at 5:06