1
$\begingroup$

I am exploring the option of using a EKF with my differential drive robot. I do not have any prior experience with kalman filters.

The robot that is under consideration has two wheel encoders for odometry input using which can be used to calculate the displacement and heading change in the odom frame. It also has two magnetic sensors placed under either ends of the robot which will provide offset distance while traveling on top of a magnetic line.

Using the odometry model I can calculate the x, y and yaw of the bot over a period of time. And the magnetic measurement provides the y and yaw in world frame. Using this paper as my reference, my state transition matrix is jacobian of the odometry model as included below.

$$A = \begin{bmatrix}1 & 0 & -\Delta \sin(\theta_k + \omega/2)\\0 & 1 & \Delta \cos(\theta_k + \omega/2)\\0 & 0 & 1 \end{bmatrix}$$

So, the system model is

$$X_k = A * X_{k-1}$$

For initial values of $(0,0,0)$, when the robot is aligned with the world frame,

$$ \begin{bmatrix} x_{k+1} \\ y_{k+1} \\\theta_{k+1} \end{bmatrix} = \begin{bmatrix}1 & 0 & -\Delta \sin(\theta_k + \omega/2)\\0 & 1 & \Delta \cos(\theta_k + \omega/2)\\0 & 0 & 1 \end{bmatrix} * \begin{bmatrix} 0 \\ 0 \\0 \end{bmatrix}$$

My measurement model will provide only y and yaw values. Hence, my H matrix and the z are as below

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

Going by the above model, if the robot is moving on top of the magnetic sensor in a straight line, my measured y and yaw will always be 0 and hence based on the equations outlined in the paper (also included above) my X value will never increment.

Any inputs to help me understand what I am missing here are highly appreciated.

[Edited]

The heading of the bot is along the x axis. So as it travels forward with 0 yaw the X will increase.

Improve this question
$\endgroup$
7
  • 1
    $\begingroup$ Can you clarify what exactly you want to know? The problem you seem to be coming up against is something called Observability. Essentially your measurement model can not observe certain types of movement. This example of moving along a straight line is a pretty common example. There is nothing you can do to fix it, except for adding some other type of sensor that can measure it. $\endgroup$
    – edwinem
    Commented Jun 25, 2020 at 18:54
  • $\begingroup$ @edwinem If Im moving in a straight line, my measurement would always be y=0 and yaw=0. Assuming I start with [0,0,0] as my initial state, my X axis value would never increment as long as my measured y and yaw are 0, regardless of the linear displacement that my odometry model captures. $\endgroup$
    – ZincFur
    Commented Jun 26, 2020 at 1:03
  • 1
    $\begingroup$ But the odometry model will increment the X state. $x_{k+1}=x_k+\delta*cos(\theta_k+\omega/2)$ So lets say both encoders counted 5 steps so $\delta=5$ and $\omega=0$. So $x_{k+1}=0+5 * cos(0+0)=5$. And it will just continue on. The error will always increase as you can't measure $X$ directly but you still have an idea of it. $\endgroup$
    – edwinem
    Commented Jun 26, 2020 at 1:45
  • $\begingroup$ @edwinem Going by the math above in my question, $x_{k+1} = 1 * x_k + 0 * y_k + (\theta_k * -\Delta \sin(\theta_k + \omega/2))$. So for the values you suggested above, $x_{k+1} = 1 * 0 + 0 * 0 + (0 * -5 * \sin(0)) = 0$. Hence $x_{k+1}$ will always be zero no matter how much the $\Delta$ increases. Is there anything Im missing here? $\endgroup$
    – ZincFur
    Commented Jun 26, 2020 at 13:40
  • 1
    $\begingroup$ You are confusing the jacobian matrix with the state transition equation. In the paper you referenced Eq 1 is what you use to update your state. It is also not even a matrix since it is non linear. The $A$ matrix you have is used to update the covariance.You can see that the prediction step consists of 2 parts in Eq 9 and 10. One is used to update your state via some function (Eq 9,1), and the other is the jacobian of that function used to update your uncertainty($A$ matrix, Eq 10) $\endgroup$
    – edwinem
    Commented Jun 26, 2020 at 14:47

2 Answers 2

Reset to default
0
$\begingroup$

I can’t open the link to the paper you put in your question, but perhaps this is the way to think about the situation. If your robot is facing along the y axis, and $\theta = 0$, then how can you get any motion in the x direction unless you first rotate $\theta$ until the wheels are not aligned with the y axis? It seems to me the equation correctly reflects the physical constraints for such a nonholonomic system.

Improve this answer
$\endgroup$
1
  • $\begingroup$ The heading of the robot is along the x axis. So when it moves with 0 yaw then it will be having only X component. The measured y and yaw are using a magnetic line strip that is placed along the X direction of the bot. I have edited the question to include this. $\endgroup$
    – ZincFur
    Commented Jun 25, 2020 at 16:42
0
$\begingroup$

The answer from @edwinem is

"You are confusing the jacobian matrix with the state transition equation. In the paper you referenced Eq 1 is what you use to update your state. It is also not even a matrix since it is non linear. The A matrix you have is used to update the covariance.You can see that the prediction step consists of 2 parts in Eq 9 and 10. One is used to update your state via some function (Eq 9,1), and the other is the jacobian of that function used to update your uncertainty(A matrix, Eq 10)"

Improve this answer
$\endgroup$

Your Answer

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

Not the answer you're looking for? Browse other questions tagged or ask your own question.