Skip to main content
Open In Colab The EKF section pushed a Gaussian belief through a U-shaped function and found that the tangent line fails where the function is flat at the mean of the belief: the linearized variance drops to zero although the true output still spreads. This section shows the same failure on a robot. A TurtleBot drives straight ahead with its heading known only roughly, and one prediction step of the EKF is compared with samples pushed through the motion model itself. The prediction uses the EKF class of the EKF section, built on filterpy’s ExtendedKalmanFilter:

​
The TurtleBot motion model

The straight drive at the start of the EKF section is a special case of the differential-drive motion model of the TurtleBot. As in nonholonomic robots, two wheels of radius rr sit at distance dd on either side of the chassis origin. The action is the pair of wheel speeds at=(uL,uR)a_t = (u_L, u_R), which set the forward speed and the turning rate, as derived there from rolling without slipping: v=r2(uL+uR),ω=r2d(uR−uL).v = \frac{r}{2}(u_L + u_R), \qquad \omega = \frac{r}{2d}(u_R - u_L). With the state s=(x,y,θ)s = (x, y, \theta) and a time step Δt\Delta t, the motion model and its Jacobian are g(at,st−1)=[x+v Δtcos⁡θy+v Δtsin⁡θθ+ω Δt],Gt=[10−v Δtsin⁡θ01v Δtcos⁡θ001].g(a_t, s_{t-1}) = \begin{bmatrix} x + v\,\Delta t\cos\theta \\ y + v\,\Delta t\sin\theta \\ \theta + \omega\,\Delta t \end{bmatrix}, \qquad G_t = \begin{bmatrix} 1 & 0 & -v\,\Delta t\sin\theta \\ 0 & 1 & v\,\Delta t\cos\theta \\ 0 & 0 & 1 \end{bmatrix}. This model moves the robot v Δtv\,\Delta t along its current heading and then turns it by ω Δt\omega\,\Delta t. It is the first-order approximation of the motion model in EKF localization, which holds the body twist constant over the step and moves the robot along the exact arc, Tt=Tt−1exp⁡([Vb] Δt)T_t = T_{t-1}\exp([\mathcal{V}_b]\,\Delta t). Expanding that arc in powers of Δt\Delta t gives xt=x+v Δtcos⁡θ−12 v ω Δt2sin⁡θ+O(Δt3),yt=y+v Δtsin⁡θ+12 v ω Δt2cos⁡θ+O(Δt3).\begin{aligned} x_t &= x + v\,\Delta t\cos\theta - \tfrac{1}{2}\, v\,\omega\,\Delta t^2 \sin\theta + O(\Delta t^3), \\ y_t &= y + v\,\Delta t\sin\theta + \tfrac{1}{2}\, v\,\omega\,\Delta t^2 \cos\theta + O(\Delta t^3). \end{aligned} The model here keeps the first-order terms and leaves out the Δt2\Delta t^2 terms, a sideways drift that the robot builds up by turning during the step. The two models agree exactly when ω=0\omega = 0. The drive below is straight, so its results are the same under either model. The action at=(uL,uR)a_t = (u_L, u_R) is in R2\mathbb R^2 and the state is in R3\mathbb R^3, so gg maps R3×R2\mathbb R^3 \times \mathbb R^2 to R3\mathbb R^3. The EKF holds the action at its given value and differentiates with respect to the state only. In the notation of the Jacobian note of the EKF section, the map is st−1↦g(at,st−1)s_{t-1} \mapsto g(a_t, s_{t-1}) from R3\mathbb R^3 to R3\mathbb R^3, so n=m=3n = m = 3 and GtG_t is 3×33 \times 3. The derivative with respect to the two wheel speeds is a 3×23 \times 2 matrix. The EKF localization section uses it to turn wheel-speed noise into motion noise. Let the robot start at the origin and drive straight down the page, uL=uRu_L = u_R, with its heading known only roughly. The heading is uncertain even when every sensor works as specified: wheel slip and small errors in the wheel radii add heading error at every step of odometry, the gyro’s bias integrates into a heading drift, and after startup, or after the robot is carried to a new spot, it has no heading reference until it sees a landmark. Write the heading as θ=−90°+ψ\theta = -90° + \psi, where ψ\psi is the heading error. The figure shows where the robot ends up. Top: a right triangle formed by a 1 m drive from the start, rotated by a heading error psi from the intended straight-down direction. The drive is the hypotenuse v dt, the forward leg is v dt cos psi, the sideways leg is v dt sin psi, and the forward shortfall v dt (1 - cos psi) is marked, with the circle of all end points dotted. Bottom: the sideways offset and the shortfall as functions of psi from -60 to 60 degrees, with their first- and second-order approximations dashed. The heading is θ=−90°+ψ\theta = -90° + \psi, so the drive of length v Δtv\,\Delta t points ψ\psi away from straight down. The drive is the hypotenuse of a right triangle, and its two legs are the sideways offset and the forward progress. Its components are x=v Δtcos⁡θx = v\,\Delta t\cos\theta and y=v Δtsin⁡θy = v\,\Delta t\sin\theta. With cos⁡(−90°+ψ)=sin⁡ψ\cos(-90° + \psi) = \sin\psi and sin⁡(−90°+ψ)=−cos⁡ψ\sin(-90° + \psi) = -\cos\psi they become x=v Δtsin⁡ψ,y=−v Δtcos⁡ψ.x = v\,\Delta t\sin\psi, \qquad y = -v\,\Delta t\cos\psi . Whatever the heading error, the end point lies on the dotted circle of radius v Δtv\,\Delta t around the start. For small ψ\psi, sin⁡ψ≈ψ\sin\psi \approx \psi and cos⁡ψ≈1−ψ2/2\cos\psi \approx 1 - \psi^2/2, so x≈v Δt ψ,y≈−v Δt+v Δt2 ψ2.x \approx v\,\Delta t\,\psi, \qquad y \approx -v\,\Delta t + \frac{v\,\Delta t}{2}\,\psi^2 . The sideways offset (green) is first order in the heading error. It grows in proportion to ψ\psi, and in the lower panel it stays on its straight line across the ±30°\pm 30° band. The shortfall in forward progress (pink) is second order. It is flat at ψ=0\psi = 0 and grows like ψ2\psi^2, and it is the same for +ψ+\psi and −ψ-\psi, so the robot always stops short of the intended 1 m and never goes past it. A small heading error moves the robot sideways much more than it shortens the drive: at ψ=10°\psi = 10° the offset is 17 cm and the shortfall 1.5 cm.

​
Sampled and EKF predictions

The code below uses the geometry of a TurtleBot3 Waffle and drives it straight down the page at 0.25 m/s for 4 s, starting from a belief that knows the position to 2 cm but the heading only to 30°30°. It predicts the new pose twice: by pushing samples of the starting pose through gg, and with one prediction step of the EKF class above. To isolate the effect of the heading, the motion noise QtQ_t is left out. Before looking at the result, evaluate GtG_t at θ=−90°\theta = -90° and decide how much of the heading uncertainty the EKF passes into the sideways coordinate xx and how much into the forward coordinate yy.
Left: the forward position after a 1 m drive as a function of the heading error, a U-shaped curve with its flat tangent at zero error and the plus or minus two sigma heading belief shaded. Right: histogram of the sampled forward positions, ending at -1 m and spread toward shorter progress, with the EKF's narrow Gaussian spike at -1 m. The left panel is the motion model along yy, drawn the same way as the U-shaped function gg in the EKF section. The curve is the forward position after the drive as a function of the heading error, y(ψ)=−v Δtcos⁡ψy(\psi) = -v\,\Delta t\cos\psi, and the shaded band is the heading belief out to two standard deviations, ±60°\pm 60°. It is the U again, with its lowest point at the mean of the belief, ψ=0\psi = 0. The EKF replaces the curve with its tangent line at the mean. That line is flat, because the Jacobian entry ∂y/∂θ=v Δtcos⁡θ\partial y / \partial\theta = v\,\Delta t\cos\theta is zero at θ=−90°\theta = -90°. This is the case of the belief centered at the bottom of the U in When linearization fails, where the linearized variance was zero. The right panel shows the result. The sampled forward position stops at −1-1 m and spreads toward shorter progress, and its mean is −0.87-0.87 m. The EKF puts all of its probability in a narrow spike at −1-1 m, only as wide as the 2 cm starting uncertainty. It reports almost no uncertainty about how far the robot went, and it is wrong. The same samples, drawn in the plane, show where the robot can end up. Sampled end positions of the drive lying on an arc of radius 1 m below the start, with the EKF's two-sigma ellipse, wide sideways and only 4 cm tall, centered at the full 1 m, and the sampled mean above it. The gray square marks the start at the origin, and the arrow shows the heading straight down the page. The dots are 3,000 of the sampled positions. They lie on an arc. Every sample gets the same command, both wheels at the same speed, so it drives straight for exactly 1 m and never turns. The samples differ only in where they point at the start: the belief knows the starting heading only to 30°, and each sample draws one heading from it and keeps it for the whole drive. A 1 m straight drive along each of those headings ends on a circle of radius 1 m around the start, and the 2 cm uncertainty in the starting position gives the arc its thickness. The dashed ellipse is the EKF’s prediction, drawn two standard deviations out from its mean. The prediction is one step from the starting belief (μ0,Σ0)(\mu_0, \Sigma_0) to (μˉ1,Σˉ1)(\bar\mu_1, \bar\Sigma_1). With μˉ1\bar\mu_1 and Σˉ1\bar\Sigma_1 restricted to their x,yx, y parts, the ellipse is the set of positions pp with (p−μˉ1)⊤Σˉ1−1(p−μˉ1)=2\sqrt{(p - \bar\mu_1)^\top \bar\Sigma_1^{-1} (p - \bar\mu_1)} = 2, and it holds about 86% of the probability of the EKF’s Gaussian. The shape of the ellipse comes from the Jacobian. With the motion noise left out, the EKF’s predicted covariance is Σˉ1=G1Σ0G1⊤\bar\Sigma_1 = G_1 \Sigma_0 G_1^\top, with G1G_1 evaluated at the starting mean μ0\mu_0. Only the third column of G1G_1, the one for θ\theta, differs from the identity. At the starting heading θ=−90°\theta = -90° that column is (v Δt,0,1)⊤(v\,\Delta t, 0, 1)^\top, because −v Δtsin⁡θ=v Δt-v\,\Delta t\sin\theta = v\,\Delta t and v Δtcos⁡θ=0v\,\Delta t\cos\theta = 0. All of the heading variance therefore goes into xx and none into yy: (Σˉ1)xx=σx2+(v Δt)2σθ2,(Σˉ1)yy=σy2,(Σˉ1)xy=0.(\bar\Sigma_1)_{xx} = \sigma_x^2 + (v\,\Delta t)^2\sigma_\theta^2, \qquad (\bar\Sigma_1)_{yy} = \sigma_y^2, \qquad (\bar\Sigma_1)_{xy} = 0. With v Δt=1v\,\Delta t = 1 m, σθ=30°=0.52\sigma_\theta = 30° = 0.52 rad and σx=σy=2\sigma_x = \sigma_y = 2 cm, the EKF’s standard deviation is 0.52 m in xx and 0.02 m in yy. The cross term is zero, so the ellipse lines up with the axes. In xx the EKF is close to the samples. Sideways, the robot ends at x=v Δtsin⁡ψx = v\,\Delta t\sin\psi, and sin⁡ψ≈ψ\sin\psi \approx \psi for small heading errors, so the tangent line stays close to the curve. The samples spread 0.46 m in xx, a little less than the EKF’s 0.52 m, because sin⁡ψ\sin\psi falls below ψ\psi for the larger heading errors in the tails of the belief. In yy the EKF is far off. The samples spread 0.17 m, more than eight times the EKF’s 2 cm, because the tangent at the bottom of the U is flat and passes none of the heading uncertainty into the forward direction. The samples show what actually happens. Each one is a starting pose that the belief allows, pushed through the motion model itself, so together they are the robot’s distribution at t=1t = 1: somewhere on the arc, on average 0.87 m forward, and about 1 m at most. The EKF describes the same moment with a Gaussian centered at the full meter that claims to know the forward position to 2 cm. That belief is overconfident twice over. Its mean puts the robot 13 cm farther forward than the samples’ mean, and its spread in yy is less than an eighth of the real one. Nothing inside the filter shows the problem: Σˉ1\bar\Sigma_1 is small, so the belief looks trustworthy, and the next measurement update gives the measurement less weight than it deserves. This is the start of the filter divergence described in the EKF section under When linearization fails. The localization section continues this example: the robot measures the range and bearing to a landmark, and the EKF uses the measurement to correct its belief.
Found an error on this page? Report an issue.