Lab 10: Grid Localization using Bayes Filter

2026-04-20T00:00:00+00:00

Overview</h2>
The robot doesn't know where it is on its own, so it has to figure out its position from ToF sensor readings, this is called localization. In this lab I implemented a Bayes filter, which keeps a running "belief" about where the robot thinks it is. Every time fresh sensor data or a control input comes in, the filter updates that belief through Bayesian inference. The goal of this lab is to get everything working in simulation before putting it on real hardware.</p>

Grid Localization Parameters</h3>
The robot's state is 3D: $(x, y, \theta)$. The arena spans roughly -5.5 to 6.5 ft in $x$, -4.5 to 4.5 ft in $y$, and $[-180°, +180°)$ in heading. Since we can't compute over a continuous space, the arena is chopped into a 3D grid of $(12, 9, 18)$ cells, each $1 \text{ ft} \times 1 \text{ ft} \times 20°$. Every cell holds a probability of the robot being there, and the whole grid sums to 1. After each update, the cell with the highest probability is our best guess at the robot's pose.</p>

Bayes Filter Architecture</h2>

The filter runs in a loop with two stages. The prediction step</strong> uses control inputs to forecast where the robot moved, and the update step</strong> corrects that guess by comparing expected sensor readings against the real ones.</p>

Algorithm</strong> Bayes_Filter( $bel(x_{t-1}),\ u_t,\ z_t$ ) 


    for all</strong> $x_t$ do</strong> 

        $\overline{bel}(x_t) = \sum_{x_{t-1}} p(x_t \mid u_t, x_{t-1}) , bel(x_{t-1})$ 

        $bel(x_t) = \eta , p(z_t \mid x_t) , \overline{bel}(x_t)$ 

    end for</strong> 

    return</strong> $bel(x_t)$</p>
</div>
Odometry Motion Model</h3>
The control input $u$ is broken into an initial rotation, a translation, and a final rotation, which together describe any transition between two poses.</p>


Odometry model parameters: $\delta_{rot1}$, $\delta_{trans}$, and $\delta_{rot2}$.</figcaption>
</figure>
$$\delta_{rot1} = \operatorname{atan2}(\bar{y}' - \bar{y},, \bar{x}' - \bar{x}) - \bar{\theta}$$</p>
$$\delta_{trans} = \sqrt{(\bar{x}' - \bar{x})^2 + (\bar{y}' - \bar{y})^2}$$</p>
$$\delta_{rot2} = \bar{\theta}' - \bar{\theta} - \delta_{rot1}$$</p>
Algorithm Implementation</h2>
Compute Control & Motion Model</h3>
compute_control()</code> pulls the three kinematic parameters out of two consecutive poses. odom_motion_model()</code> then evaluates how likely a given transition is by plugging the actual and expected controls into Gaussians.</p>
import</span> math</span></span>
import</span> numpy</span> as</span> np</span></span>
</span>
def</span> compute_control</span>(cur_pose, prev_pose):</span></span>
    x_prev, y_prev, yaw_prev</span> =</span> prev_pose</span></span>
    x_cur, y_cur, yaw_cur</span> =</span> cur_pose</span></span>
</span>
    delta_trans</span> =</span> math.hypot(x_cur</span> -</span> x_prev, y_cur</span> -</span> y_prev)</span></span>
    delta_rot_1</span> =</span> math.degrees(math.atan2(y_cur</span> -</span> y_prev, x_cur</span> -</span> x_prev))</span> -</span> yaw_prev</span></span>
    delta_rot_1</span> =</span> mapper.normalize_angle(delta_rot_1)</span></span>
</span>
    delta_rot_2</span> =</span> yaw_cur</span> -</span> yaw_prev</span> -</span> delta_rot_1</span></span>
    delta_rot_2</span> =</span> mapper.normalize_angle(delta_rot_2)</span></span>
</span>
    return</span> delta_rot_1, delta_trans, delta_rot_2</span></span>
</span>
def</span> odom_motion_model</span>(cur_pose, prev_pose, u):</span></span>
    rot1, trans, rot2</span> =</span> u</span></span>
    delta_rot1, delta_trans, delta_rot2</span> =</span> compute_control(cur_pose, prev_pose)</span></span>
</span>
    p1</span> =</span> loc.gaussian(delta_rot1, rot1, loc.odom_rot_sigma)</span></span>
    p2</span> =</span> loc.gaussian(delta_trans, trans, loc.odom_trans_sigma)</span></span>
    p3</span> =</span> loc.gaussian(delta_rot2, rot2, loc.odom_rot_sigma)</span></span>
</span>
    return</span> p1</span> *</span> p2</span> *</span> p3</span></span></code></pre>Prediction Step</h3>
This is the heavy computation stage. The grid has $12 \times 9 \times 18 = 1944$ cells, and a naive implementation considers every (previous, current)</em> pair, so each tick loops over $1944^2 \approx 3.8$ million transitions. To keep things tractable, I skip any previous cell with belief below $0.0001$, since those barely contribute to the sum. The tradeoff is a small accuracy hit for a huge speedup. After accumulating the probabilities I normalize so the grid stays a valid distribution.</p>
def</span> prediction_step</span>(cur_odom, prev_odom):</span></span>
    u</span> =</span> compute_control(cur_odom, prev_odom)</span></span>
    loc.bel_bar</span> =</span> np.zeros((mapper.</span>MAX_CELLS_X</span>, mapper.</span>MAX_CELLS_Y</span>, mapper.</span>MAX_CELLS_A</span>))</span></span>
</span>
    for</span> cx</span> in</span> range</span>(mapper.</span>MAX_CELLS_X</span>):</span></span>
        for</span> cy</span> in</span> range</span>(mapper.</span>MAX_CELLS_Y</span>):</span></span>
            for</span> ca</span> in</span> range</span>(mapper.</span>MAX_CELLS_A</span>):</span></span>
                if</span> loc.bel[cx, cy, ca]</span> ></span> 0.0001</span>:</span></span>
                    prev_pose</span> =</span> mapper.from_map(cx, cy, ca)</span></span>
                    for</span> cur_cx</span> in</span> range</span>(mapper.</span>MAX_CELLS_X</span>):</span></span>
                        for</span> cur_cy</span> in</span> range</span>(mapper.</span>MAX_CELLS_Y</span>):</span></span>
                            for</span> cur_ca</span> in</span> range</span>(mapper.</span>MAX_CELLS_A</span>):</span></span>
                                cur_pose</span> =</span> mapper.from_map(cur_cx, cur_cy, cur_ca)</span></span>
                                prob</span> =</span> odom_motion_model(cur_pose, prev_pose, u)</span></span>
                                loc.bel_bar[cur_cx, cur_cy, cur_ca]</span> +=</span> prob</span> *</span> loc.bel[cx, cy, ca]</span></span>
</span>
    loc.bel_bar</span> /=</span> np.sum(loc.bel_bar)</span></span></code></pre>Sensor Model & Update Step</h3>
I vectorized the update step to avoid looping over every cell. By reshaping the raw sensor readings and broadcasting them against the cached mapper.obs_views</code> array, NumPy computes all 18 sensor likelihoods across the entire grid in one shot. This is a huge speedup over the nested-loop version if I used the sensor_model function and it still implements the same Bayesian update:</p>
$$p(z_t \mid x_t, m) = \prod_{k=1}^{18} p(z_t^k \mid x_t, m)$$</p>
def</span> update_step</span>():</span></span>
    actual_obs</span> =</span> loc.obs_range_data.flatten().reshape(</span>1</span>,</span> 1</span>,</span> 1</span>, mapper.</span>OBS_PER_CELL</span>)</span></span>
    likelihoods</span> =</span> loc.gaussian(mapper.obs_views, actual_obs, loc.sensor_sigma)</span></span>
    cell_likelihoods</span> =</span> np.prod(likelihoods,</span> axis</span>=</span>3</span>)</span></span>
</span>
    loc.bel</span> =</span> cell_likelihoods</span> *</span> loc.bel_bar</span></span>
    loc.bel</span> /=</span> np.sum(loc.bel)</span></span></code></pre>Simulation & Results</h2>
The video below shows the filter localizing along a pre-planned rectangular path. Red is the raw odometry estimate, green is ground truth, and blue is the Bayes filter's belief. The odometry drifts badly, you can see it shoot off the map entirely in the bottom right and overshoot again near the top, but the blue belief tracks green closely the whole way around the obstacle. The white cells behind the robot visualize the belief grid, brighter means higher probability, and I'm ignoring anything below $0.0001$.</p>
</iframe>
Bayes simulation tracking ground truth vs. odometry.</figcaption>
The filter works noticeably better near obstacles. This is probably because ToF sensors are more stable at short ranges, so readings there carry more useful information. In the open middle of the arena there aren't many nearby things to lock onto, and the belief gets a bit fuzzier. Still, across the full trajectory the Bayes estimate consistently tracked ground truth far better than odometry alone.</p>
Collaboration</h2>
I referenced Lucca Correia's site while debugging the filter and putting together the video demo. I also used lec 17-20 of the class lecture to implement my Bayes Filter.</p>

Home - Algorithms

Lab 10: Grid Localization using Bayes Filter

Algorithm Implementation</h2>

Collaboration</h2> I referenced Lucca Correia's site while debugging the filter and putting together the video demo. I also used lec 17-20 of the class lecture to implement my Bayes Filter.</p>

Collaboration</h2>
I referenced Lucca Correia's site while debugging the filter and putting together the video demo. I also used lec 17-20 of the class lecture to implement my Bayes Filter.</p>