Pavone Shows Single Landmark Observation Collapses Robot Map Drift

Pavone展示单次地标观测即可消除机器人建图漂移 由于增广状态矩阵中机器人轨迹与地标估计共享非零互协方差，闭环检测期间对单个地标的再次观测会级联缩小整个地图的累积不确定性。 Marco Pavone教授首先通过解决对应关系未知的数据关联问题，在纯机器人定位与完整同步定位与建图（SLAM）之间架起桥梁。他展示了极大似然估计和马氏距离如何使机器人能够将测量返回值分配给环境地标，并将此方法与蒙特卡洛定位等粒子滤波方法进行了对比。在过渡到EKF SLAM时，他构建了结合机器人位姿与静态地标坐标的增广状态向量，解释了测量子雅可比矩阵的计算机制，并阐明了非对角协方差项如何使闭环检测将确定性传播到整个地图。 在同步定位与建图（SLAM）中，将环境地标与机器人位姿一同估计会使状态向量从3个平面独轮车变量扩展到1,000至10,000个以上的维度，对于n个点地标，形成大小为3 + 2n的增广状态向量。 (47:27) 未知对应关系下的数据关联通过使用马氏距离的极大似然估计来解决，在计算上的不可行性与错误关联被当作真实情况处理时的脆弱性之间进行权衡。 (0:05) 闭环检测利用了(3 + 2n) x (3 + 2n)协方差矩阵内的全互协方差耦合，使得单次消除位姿不确定性的观测能够同时缩小所有先前建图地标的不确定性。 (70:47) 与在对称环境中表示多峰分布的粒子滤波器（蒙特卡洛定位）不同，扩展卡尔曼滤波强制使用单峰高斯分布，这使得标准EKF主要适用于基于可靠初始位姿的局部跟踪。 (39:13)

Professor Marco Pavone examines the transition from known-map localization to Simultaneous Localization and Mapping (SLAM), demonstrating how correspondence estimation and state augmentation reshape robotic tracking. While maximum-likelihood data association introduces operational brittleness, the dense cross-covariances of EKF SLAM allow loop closure events to retroactively shrink uncertainty across the entire map. Maximum Likelihood and Mahalanobis Data Association (0:05) In real-world localization, robots lack an oracle to specify which landmark produced an observed sensor reading. Instead, systems infer correspondences by maximizing measurement likelihood. Because evaluating all joint assignments of a dense sensor scan against an entire map scales exponentially, standard implementations decouple the problem to evaluate each measurement beam independently. Linearizing the measurement model via an Extended Kalman Filter (EKF) yields a closed-form Gaussian distribution over the expected observation, whose innovation covariance combines the robot's projected pose uncertainty with intrinsic sensor noise. Pavone explains that this optimization is equivalent to minimizing the Mahalanobis distance, which scales geometric offsets by directional variance rather than treating spatial axes uniformly. However, committing to hard maximum-likelihood assignments makes localization brittle: if an assignment error occurs, the algorithm accepts the incorrect correspondence as ground truth, corrupting subsequent state updates. In practice, systems mitigate this risk through outlier rejection, high-density landmark tracking, or multi-hypothesis filtering. Representational Limits: EKF Versus Particle Filters (29:54) The lecture contrasts parametric estimators with non-parametric methods like Monte Carlo Localization (MCL). MCL represents the robot's belief using discrete particles that are propagated through motion dynamics, weighted by measurement probabilities, and resampled with replacement. While conceptually simple, practical implementations suffer from computational bottlenecks during naive iteration and face the risk of particle starvation, where repeated resampling collapses the population onto an incorrect state. To survive particle starvation or kidnapping, systems periodically inject randomly distributed particles into the map. Despite these operational caveats, particle filters accommodate multimodal beliefs that break standard Gaussian filters. In symmetric environments, such as identical rooms or corridors, sensor observations support multiple distinct physical locations simultaneously. A unimodal EKF averages these distinct peaks, placing its mean in an intermediate space where the robot cannot physically reside. Consequently, Pavone emphasizes that standard EKFs are restricted to local tracking from a known starting pose, whereas particle filters succeed at global localization from scratch. Formulating SLAM and State Vector Augmentation In SLAM, the map moves from a known conditional variable into the active state vector being estimated alongside the robot's pose. While online SLAM recursively estimates the current pose and the map, full SLAM estimates the vehicle's entire historical trajectory. This structural shift causes a massive dimensionality explosion: a planar unicycle localization model tracking three kinematic variables expands in SLAM to a state vector of dimension 3 + 2n, where n represents hundreds or thousands of environmental point landmarks. Under the underlying graphical model, errors in the robot's path and errors in landmark locations are intimately correlated. EKF SLAM manages this coupling under the physical assumption that the operating environment is static. Because landmarks remain stationary over time, the non-linear state transition function operates exclusively on the 3-variable kinematic pose, while landmark coordinates pass through an identity mapping with zero process noise. Computational Mechanics, Initialization, and Loop Closure (59:12) Computing measurement Jacobians across thousands of state dimensions is optimized by recognizing that any single observation depends on only five variables: the robot pose and the coordinates of the detected landmark. The algorithm evaluates a compact 2x5 sub-Jacobian and routes it into the full measurement Jacobian using a sparse projection matrix. Newly observed landmarks are initialized through an inverse sensor model anchored to the robot's current pose mean, with initial landmark uncertainties set to infinity. The dense structure of the augmented covariance matrix enables the primary strength of EKF SLAM: loop closure. Because off-diagonal covariance terms preserve cross-correlations between the vehicle's trajectory and every mapped feature, re-observing an early landmark after a long period of drift immediately constrains the robot's pose. Through the Kalman gain, this pose correction cascades across all off-diagonal blocks, simultaneously shrinking uncertainty ellipses for all previously mapped landmarks. For unknown correspondences, candidates are evaluated by comparing normalized Mahalanobis distances against an empirical threshold parameter to distinguish existing landmarks from new features. Although EKF SLAM provides an analytically tractable formulation where correlated errors enable global map refinement, its reliance on hard data association leaves it vulnerable to spurious landmark generation and linearization errors. Practical deployment requires pairing the estimator with distinctive feature extraction and outlier rejection, while subsequent lectures explore non-parametric alternatives such as FastSLAM.

斯坦福大学 Marco Pavone 教授系统阐述了机器人从已知关联的纯定位向扩展卡尔曼滤波同步定位与建图（EKF SLAM）的理论演化，解析了如何将未知的环境地标与机器人位姿整合进增广状态向量。 未知数据关联与极大似然估计的脆性 (0:05) 在纯定位研究中，传统模型常假设存在能直接指明测量源自哪座地标的神谕关联变量 $c_t^i = j$；但真实场景中系统必须自主估计关联。为规避联合批处理评估所导致的组合爆炸，工程上常将其解耦为单次测量的极大似然估计（MLE）。该方法选择令当前测量条件概率最大化的地标，在数学等价形式上即为最小化马氏距离（Mahalanobis Distance）。马氏距离通过协方差矩阵的逆来缩放几何欧氏距离，准确反映出各向异性不确定性下的统计标准差偏离。 参数化高斯滤波与非参数化粒子滤波的权衡 (29:54) 以 EKF 为代表的参数化方法强制使用单模态高斯分布，仅适用于局部跟踪与增量修正。面对对称长廊等测量完全不可辨的场景，EKF 的后验均值容易坍塌至两间对称房间的正中心，这一均值位置恰好是机器人绝对不可能存在的实体障碍区域。相比之下，蒙特卡洛定位（MCL）利用成百上千个粒子对后验分布进行离散采样，不仅能在全局未知初始位姿下灵活表达多模态置信度，还能在观测到非对称结构时迅速收敛。但粒子滤波极易遭遇计算性能瓶颈与粒子匮乏（Particle Starvation）失效，实际部署中常需定期注入随机粒子以维系信念多样性并应对绑架问题。 增广状态向量与静态环境下的稀疏雅可比技巧 (51:56) 进入 SLAM 范畴后，环境地图从已知先验转变为待估计的状态变量，使原本仅含 3 个位姿参数的轮式模型瞬间膨胀为包含数千乃至上万个连续变量的高维后验分布。EKF SLAM 构造了维度为 $3+2n$ 的增广状态向量 $y_t$，直接打包了机器人二维位姿及所有 $n$ 个地标的笛卡尔坐标。基于静态环境假设，地标不随时间发生自主运动，故状态转移噪声矩阵仅在左上角 $3\times 3$ 的位姿运动学子块具有非零项。在观测模型线性化阶段，针对具体地标 $j$ 的距离-方位角测量仅依赖 5 个连续变量，系统通过构造 $5 \times (3+2n)$ 的稀疏投影矩阵 $F_{x,j}$ 将 $2\times 5$ 的低维子雅可比矩阵映射入全状态雅可比矩阵 $H_t$，有效消除了全维度求导的算力冗余。 全协方差耦合与回环检测的不确定性级联收缩 (67:30) 在无先验地图启动时，算法将初始位姿设定为全局原点并赋予零协方差，未观测地标的初始不确定性则置为无穷大，并在初次捕获时经由反向传感器模型计算均值坐标。由于增广协方差矩阵为全满矩阵，非对角元素完整记录了机器人位姿与每座地标之间的强误差交叉相关性。当机器人在开环运动中因里程计累积漂移而膨胀至巨大不确定性后，重新观测到起始地标所触发的回环检测（Loop Closure）会瞬间将机器人位姿锁定在狭窄区间内；这种单点测量带来的位姿方差坍缩会沿着全满增广协方差矩阵的非对角项跨变量级联传播，同步削减全局所有已建图地标的不确定性包络。 未知环境下的地标动态增广与鲁棒性挑战 (75:21) 当面对未知数据关联时，EKF SLAM 必须动态扩张增广状态向量。算法对每个测得的特征计算候选现有地标的马氏距离，并预先设定固定阈值 $\alpha$ 作为虚拟新地标假设。一旦测量与所有已知地标的马氏距离全数超过 $\alpha$，系统即将其判定为全新地标并扩充状态维度。然而，硬性判定极易因传感器野值而误将噪声登记为虚假地标，从而破坏估计稳定性。因此，工程落地必须依赖前端高区分度特征提取以及后端严格的外点剔除流程来抑制关联脆性。 EKF SLAM 通过增广状态向量与马氏距离度量，优雅融合了连续几何估计与离散数据关联，其核心优势在于回环约束对全协方差矩阵的级联收敛效应。但该架构在本质上受限于静态环境假设、高斯单模态拟合约束，以及硬关联误判引发的状态发散隐患，未来课程将进一步向粒子滤波 SLAM（FastSLAM）和多传感器融合演进。

[48:40] Prof. Marco Pavone: If at some point I'm able to reduce the uncertainty about where I am, I can also immediately reduce the uncertainty about the map. [63:49] Prof. Marco Pavone: It's natural for a SLAM algorithm to take the initial pose as the origin of the coordinate system. Prof. Marco Pavone: The correspondence of measurement to landmarks is actually a combinatorial problem.

[0:05] Cool. All right. So, today we're going [0:07] to continue our discussion about [0:10] localization. [0:12] And then we will move to the topic of [0:14] simultaneous localization and mapping. [0:16] In particular, if you remember, a key [0:18] assumption we made the last time in [0:20] order to solve the localization problem [0:22] was to assume that [0:24] we know the correspondence variables, [0:27] which means that an oracle tells us [0:29] every time we see we have a measurement [0:32] to which landmark that measurement [0:35] corresponds to. [0:37] And this was captured, if you remember, [0:38] through the notion of a correspondence [0:40] correspondence variable. [0:42] In particular, we say that the [0:44] uh c t i is equal to j if measurement i [0:49] taken at time t corresponds to landmark [0:52] j. Say to the point j in a map that for [0:57] now we assume is given to us. [0:59] The natural question though is [1:02] uh who gives us these correspondence [1:05] variables. In general, indeed, they're [1:07] not known and need to be estimated as we [1:12] estimate our pose [1:14] within a given map. [1:16] The way to do it is to treat [1:20] actually to determine the identity of a [1:21] landmark during localization via maximum [1:24] likelihood estimation. [1:26] Whereby one first determines the most [1:28] likely value of the correspondence [1:30] variable c t and then it takes it for [1:32] granted. [1:34] Now, this has some ramifications. First [1:36] of all, [1:37] the idea is to estimate so to take the [1:40] value for the correspondence variable [1:43] that best explain the data that we have [1:45] observed. That's reasonable. But then we [1:48] take this value for granted, [1:49] which means that if we make a mistake in [1:53] the correspondence, [1:54] we're fully committed to that choice and [1:57] so we carry this mistake over over in [2:00] the subsequent time steps. [2:02] So [2:05] using maximum likelihood is generally a [2:08] good idea because it leads as we will [2:09] see to a fairly tractable algorithm to [2:12] estimate the correspondence variables [2:14] but introduces some brittleness in the [2:17] localization algorithm due to the fact [2:19] that we might commit ourselves to [2:21] correspondences that that are erroneous. [2:23] Which means that [2:25] you need to add in general some [2:28] additional levels of robustness, for [2:30] example, in terms of outlier rejection [2:32] schemes. Or simply you track many [2:36] different landmarks so that even if you [2:37] have a few mistakes here and there is [2:39] not catastrophic during your [2:41] localization process. [2:43] But having said that [2:47] we [2:48] the idea as I said to [2:51] estimate the [2:53] correspondence variables is to pick [2:58] the assignments from measurements to [3:03] landmarks [3:05] that makes the measurements as likely as [3:09] possible. [3:10] M is the map that we assume is given. UT [3:14] is the history of controls. Z1 [3:17] to T-1 is the history of measurements. [3:20] And the C1 to T is the history of [3:24] assignments for the correspondence [3:25] variable. [3:26] The correspondence variable from time [3:29] step one to time step uh [3:32] T-1 [3:34] have been determined in a previous [3:37] iteration of the estimation procedure. [3:40] What we're trying to understand is how [3:42] to select the assignment at time step T. [3:45] >> Is there a way to incorporate Is there a [3:46] different algorithm that uses instead of [3:49] that uses a probability distribution for [3:51] C1 through T? [3:52] >> Yeah, there are So this is the most [3:54] basic one. For example, a more advanced [3:56] version is a a multi-hypothesis [4:00] algorithms whereby you keep [4:03] multiple hypotheses [4:05] about your localization, each one [4:07] corresponding to a given history of the [4:10] correspondence variables, and then [4:11] they're mixed probabilistically. [4:18] Well, you [4:20] keep this error [4:22] as time move forward. That's exactly [4:24] what I was saying before. [4:26] Now, having said that, [4:29] in general you have many landmarks, so [4:31] even if you screw up with a landmark, [4:34] since you're also observing many other [4:35] landmarks, the localization process is [4:38] still can be reasonable. But second of [4:40] all, you can add additional layers of [4:42] robustness to this a basic scheme by [4:46] either reason about multiple hypotheses [4:49] for this correspondence variables, so [4:50] have basically a mixture localization [4:53] algorithm, or some other outlier [4:55] rejection, and so on. [4:56] We I'm just going to provide references [4:58] for those methodologies, but your point [5:00] is very valid. [5:02] Anyway, so the point here is that [5:04] we say, "Okay, if CT is given is equal [5:07] to some [5:09] assignment, what is the probability of [5:11] the measurement that that I just [5:12] observed?" I do that for all possible [5:15] assignments of measurement I to landmark [5:17] J, and I pick the assignment that [5:19] maximizes the likelihood of the [5:21] measurement that I have observed. So, [5:23] it's fairly reasonable. [5:25] I consider all possible assignments. [5:27] I pick the one that best explain my [5:30] current measurement as captured by the [5:33] conditional distribution of the [5:34] measurement with respect to previous [5:35] assignments, previous measurements, and [5:36] previous controls. [5:38] So, the intuition should be fairly [5:41] uh [5:42] straightforward. [5:44] Now, the challenge is that uh [5:47] um there are exponentially many uh [5:51] assignments. So, CT So, remember that [5:54] the ZT, the measurement, indeed uh [5:57] uh entails many possible measurements at [6:00] time T. For example, if you have a lidar [6:02] sensors, you have a point cloud. [6:05] So, ZT is indeed a vector [6:08] with many components [6:11] including ZTI. [6:14] And we want to find an assignment for [6:15] each one of these components to a [6:16] landmark. And the number of [6:18] possibilities is exponential [6:20] in the number of measurements, which in [6:22] general makes the direct computation of [6:26] the maximum likelihood extremely [6:29] computationally intense. [6:31] So, typical [6:33] approach is to [6:37] use an approximation, and indeed perform [6:40] maximum likelihood estimation separately [6:44] per each [6:46] measurement. [6:48] So, instead of a processing all [6:49] measurements in ZT in batch, [6:53] I process each assignment uh [6:56] uh [6:57] one by one. [6:58] So, we look at actually maximum [7:00] likelihood with respect to the specific [7:02] measurement I as opposed to the entire [7:04] vector of measurements. [7:07] Any questions about this? [7:12] And now the optimization is no longer in [7:15] terms of CT, [7:17] but becomes an optimization in terms of [7:19] a CTI. [7:22] So, for each measurement CTI, I try to [7:25] find a correspondence from measurement [7:27] in ZTI to landmark J through the [7:29] correspondence variable CTI such that [7:33] the likelihood of that measurement [7:35] conditional on the history of [7:37] measurement controls and conditional on [7:40] a specific value of the assignment for [7:41] CTI is maximized. [7:45] Is this measurement related to landmark [7:47] one? [7:48] If that is the case, what would be the [7:49] probability of observing a measurement [7:50] that I've just observed? .3 Is this [7:54] measurement related to landmark uh [7:56] two? If that is the case, what is the [7:59] probability of observing a measurement [8:00] that I've just observed? .5 Okay, so [8:03] it's better to associate this [8:05] measurement with landmark two. [8:06] That's is what we're saying. [8:08] So, the only uh additional challenge is [8:11] to represent [8:14] this probability distribution in an [8:16] analytical form, so that actually we can [8:18] solve this optimization problem. [8:21] To do so, in the EKF spirit, what we do [8:23] is to use linearizations. [8:26] The mathematics, as usual, [8:29] tend to be tedious, not complicated, but [8:32] tedious. [8:33] So, we'll just give you the high-level [8:34] idea [8:35] and I skip the mathematical derivation [8:37] is not particularly insightful. In [8:39] particular, uh the idea, in order to [8:43] compute [8:44] uh um [8:46] this probability in a uh effective way, [8:49] is to use the theorem of total [8:51] probability. So, this probability can be [8:54] written as the sum, or actually the [8:57] integral, of the company the conditional [8:59] probability with respect to XT [9:01] times the probability of XT given all [9:03] the other conditioning variable. So, [9:04] basically, I'm applying the total [9:05] probability law with respect to the [9:08] uh [9:09] uh state variable XT. [9:11] This is just as a total probability [9:12] theorem. [9:14] And then, the derivation will be very [9:16] similar to the derivation of the common [9:18] filter and EKF filter. [9:22] This probability is uh what? [9:26] Probability of uh distribution over the [9:28] current state [9:30] conditional on measurement up to time T [9:33] minus one and controls up to time T. [9:37] This is the definition of a prediction [9:39] belief. [9:42] This is [9:44] the measurement model, [9:46] which uh we assume [9:48] is uh [9:49] uh the measurement process we're [9:50] assuming is affected by uh a Gaussian by [9:54] Gaussian noise. [9:55] So, this probability distribution is [9:58] indeed a Gaussian distribution with a [10:00] variance given by QT, the noise [10:03] variance, and mean given by uh [10:06] uh the um uh measurement model equation. [10:12] The belief is also Gaussian. In the EKF [10:15] algorithm, the belief is always uh, [10:17] treated as a Gaussian. [10:19] We saw last time that when we uh, [10:22] multiply two Gaussians and then [10:24] integrate, we still get a Gaussian. [10:28] The trick is, okay, so we get a [10:30] Gaussian. What is the mean and what is [10:32] the variance with respect to the means [10:35] and variances of the two uh, Gaussians [10:39] that we multiply? That's These are the [10:40] steps that I'm skipping. [10:43] You do a couple of pages, well, maybe [10:45] one page of a quite tedious algebraic [10:47] derivation. [10:49] So, what you get [10:50] is that uh, the um, [10:53] uh, [10:55] probability of ZTI with respect to all [10:58] the assignments, correspondence [10:59] variables from time one to T, [11:02] can be written as a Gaussian [11:05] random variable [11:06] with [11:08] variance given by the sum of the noise [11:10] variance and the sum of the variance due [11:14] to the uncertainty about where the robot [11:16] thinks it is. [11:18] This is the usual um, [11:20] measurement bias that we use in the [11:22] common filter. [11:25] The variance regarding a measurement [11:27] is due to two two uh, factors. We're [11:30] uncertain where we are. [11:33] And that is reflected in our in our [11:35] uncertainty with respect to where the [11:37] landmark could be. [11:38] And it On top of that, we have [11:40] measurement noise. [11:42] The overall variance is the sum of the [11:43] two. [11:44] The mean is uh, the um, [11:49] um, [11:51] measurement that we will get if the [11:54] state would be the value of the state [11:56] would be equal to the most likely value [11:58] given by the mean mu bar T. [12:02] So, for intuition, this should be quite [12:04] intuitive. The full derivation is a [12:05] little bit tedious. [12:07] But anyway, so the point is that then [12:09] what you do is to plug this Gaussian, [12:11] you replace this uh, probability [12:13] distribution that is difficult to work [12:15] with with this Gaussian. [12:18] And in order to solve for the [12:19] assignments, then what do you do? [12:22] So, for each possible value of a CTI [12:31] say CTI equal one [12:35] you have this Gaussian. [12:38] CTI [12:40] equal two [12:43] you have [12:45] this Gaussian. [12:50] The [12:51] horizontal axis represent the [12:52] measurement. I have observed this [12:55] measurement. [12:58] So, should we assign [13:01] for this the measurement a CTI? [13:07] Should I assign this measurement to [13:09] landmark two to landmark one? [13:12] According to this procedure. [13:14] We pick the assignment that maximizes [13:18] the data likelihood. [13:20] So, which one should I pick? [13:22] I mean, intuitively. [13:24] Two, because [13:26] assuming that [13:28] correspondence variable evaluates to [13:30] two, then the distribution for the [13:33] measurement will be this Gaussian. And [13:35] so, the probability of observing the [13:37] measurement that I've just observed [13:40] will be this one here. [13:42] Here will be this value here. [13:45] This is higher, so I consider two. [13:51] So, what do you do practically? [13:53] You compute these quantities that you [13:55] compute anyway when you do EKF [13:58] localization with the [13:58] non-correspondences. [14:00] But now to estimate the correspondences, [14:02] you plot all these Gaussians, one for [14:05] each possible [14:06] assignment [14:08] of the correspondence variable. [14:10] And then you pick for the [14:12] correspondence value [14:14] the value that maximizes the [14:16] probability. [14:26] Yeah, but that actually could be [14:28] correct, right? [14:29] It's very possible that [14:31] Oh, [snorts] you're saying that the [14:33] the same measurement corresponds to two [14:35] landmarks? [14:37] Or the two landmarks correspond to the [14:39] same measurement. [14:41] Yeah, but sometimes that that that is [14:43] possible is not incorrect. [14:46] That you have a measurement that [14:47] corresponds to [14:49] two measurements that correspond to the [14:51] same landmark. Think about like a wide [14:53] obstacle and then you have a lighter [14:54] sensor and then you have a return. It's [14:57] very possible that two returns are [14:59] actually reflections from the same [15:01] obstacle, well, point landmark. [15:06] All right, so having said that, then the [15:08] EKF localization algorithm with unknown [15:11] correspondences is identical to the EKF [15:15] localization algorithms with known [15:16] correspondences. The only difference is [15:18] that we have to do this additional [15:21] maximum likelihood estimation for the [15:23] correspondence variable. How that works? [15:25] So, the first part, the part whereby we [15:29] compute the prediction belief [15:31] is exactly the same. The prediction [15:33] belief does not depend on the [15:34] measurement. [15:36] So, it makes sense that correspondence [15:37] variable doesn't play any role there. In [15:39] particular, as usual, the prediction [15:41] belief is computed by applying the state [15:45] transition model to the most likely [15:48] state as captured by the mean of the [15:50] belief at time t minus one. [15:53] And the uncertainty about the prediction [15:54] belief, as usual, is due to two [15:56] components, uncertainty about where we [15:58] are at time t minus one as captured by [16:02] the belief at time t minus one, plus [16:04] motion uncertainty as captured by the [16:08] covariance matrix RT. So, this is as [16:11] before. [16:13] The difference is that now [16:16] Um, do process it measurement [16:18] sequentially as in the EKF localization [16:21] algorithm, [16:22] but we don't know anymore how to [16:23] associate a measurement to a landmark. [16:26] So, what we do is to [16:28] uh, first [16:30] consider all possibilities. [16:33] So, the possibility that uh, [16:35] um, [16:36] uh, [16:37] measurement I corresponds to any [16:39] possible landmarks. [16:41] And therefore, each one of those [16:42] possibilities I compute the related [16:44] quantities in terms of [16:46] If that would be the case, what would be [16:47] the expected measurement and what would [16:49] be the expected uncertainty? [16:52] Again, I don't know to which [16:55] um, [16:56] landmark measurement I corresponds to. [16:59] So, what we do in the first step is to [17:01] say, [17:03] "Okay. [17:04] So, let's associate a measurement of ZTI [17:09] to landmark one. [17:13] And if this is the case, what would be [17:16] the [17:17] um, [17:19] um, [17:22] ZTI, I say, one. [17:26] What would be the measurement that I [17:28] would expect and what would be the [17:30] associated uncertain uncertainty? [17:34] If instead we associate a ZTI [17:39] to landmark two, [17:44] what would be [17:46] the expected measurement and the [17:49] associated uncertainty? And so on and so [17:52] forth. [17:53] Each one corresponds to a Gaussian. [17:57] Once I do [17:59] uh, [18:00] the computation for each landmark, then [18:02] I assign measurement I to the landmark J [18:06] that makes my data [18:08] most likely. [18:10] So, whereby I closer to the mean of the [18:12] corresponding Gaussian in a way. [18:16] This is exactly the [18:18] the correspondence problem that was [18:20] described in the first in the previous [18:22] two slides. [18:25] Once we make [18:26] >> [snorts] [18:26] >> the correspondence assignment, then [18:28] everything else is the same. [18:32] We commit to that correspondence [18:33] assignment, and then I as usual compute [18:37] the common gain. I update the belief [18:40] about the expected pose by blending [18:45] the [18:46] expectation of the previous belief and a [18:49] linear combination of [18:52] what I expect [18:54] I should have seen and what I've seen. [18:57] And then update the covariance [18:58] accordingly. So, the only difference is [19:01] here. [19:03] Does this step make sense? [19:06] I don't know a priori which landmark I [19:08] should associate a measurement to, so [19:10] consider all possible possibilities. [19:12] For each one, I ask myself, "Well, if [19:16] measurement I corresponds to landmark K, [19:18] how likely the measurement that I [19:21] I have [19:23] observed [19:24] would be?" That's what is done here. I [19:27] do it for all the possible associations [19:30] to landmark, and I pick then association [19:32] that makes [19:33] the data the most likely possible. [19:41] So, this part [19:42] tends to be a little bit confusing [19:45] because now the algorithm has a lot of [19:46] moving parts, but just trying to stress [19:48] that in terms of [19:51] changes with respect to the previous UKF [19:53] localization algorithm, there is very [19:54] little. [19:55] The only change is really here. Then now [19:59] I have to consider all possible [20:00] hypotheses, and then I pick the one that [20:04] best explain the data. [20:08] So, practically if [20:11] um [20:16] I see a door, there is no I don't know [20:18] there is also a door over there. [20:19] Uh [20:20] yeah, I I a table. [20:22] So, I get the measurement about the [20:23] return. [20:25] The question is uh [20:26] um did I see a table or not? So, do I [20:30] associate this measurement to that table [20:32] or to that podium? [20:35] Basically, this procedure says that [20:36] well, if I associate this measurement to [20:40] that podium to the table, I will have a [20:42] certain probability. If I associate it [20:43] with that with respect to that podium, I [20:45] will have a lower probability than [20:46] associating the measurement to that [20:48] landmark. [20:52] Any questions? [20:56] So, let's pause because if this is not [20:58] clear, then UKS LAM becomes even less [21:02] clear. [21:07] So, now we have basically three building [21:09] blocks. First, we propagate forward [21:12] our estimate about the pose by using the [21:15] prediction step. [21:17] The second module is to correct our [21:20] prediction by using measurement. [21:22] The third module is in order to correct [21:27] by using our prediction by using a [21:29] measurement, I need to associate a [21:31] measurement to a landmark. And this is [21:32] what is done here. [21:34] So, a lot of lines, but in reality, we [21:36] are doing three main operations. [21:41] Cool. As I said last time, so in the [21:43] description of the algorithm, I consider [21:46] point landmarks, but one can use any [21:48] other [21:49] geometric primitives [21:50] as a landmark. In particular, a common [21:53] geometric primitive that is used for [21:55] localization, and actually the one that [21:57] you will be using your homework is a [21:59] line. [22:01] Or more specifically, as discussed last [22:03] time with one of the students, will be a [22:05] line segment. [22:07] A line is then parameterized by [22:10] uh two parameters R [22:13] R and alpha. [22:15] This will be your measurement. [22:18] Another comment is that uh sometimes, in [22:21] order to uh associate a measurement with [22:24] uh uh [22:26] a landmark we associate [22:29] the [22:31] a measurement with the landmark that [22:34] minimizes Mahalanobis distance. So, what [22:37] is Mahalanobis distance? [22:39] Mahalanobis distance [22:42] is an interesting concept. [22:49] So, assume that you have a random [22:51] variable [22:52] with mean mu. [23:00] Oops. [23:01] So, this is not a dot. [23:03] This is just a point. [23:05] This is mu and this is uncertainty [23:08] ellipse. [23:11] And I want to understand how close [23:15] a given realization of that random [23:17] variable [23:20] is, meaning in terms of how many sigma [23:23] sigma [23:25] how many sigmas is a realization from [23:28] the expectation. [23:31] So, in general, when you try to So, the [23:33] expectation is a point. [23:35] If you want to measure the distance [23:36] between these two points, in general, [23:38] you will be using Euclidean distance. [23:41] But now we're reasoning in terms of the [23:42] distance with respect to a random [23:44] variable. [23:45] Which means that if we don't use the one [23:46] don't want to use the notion of [23:49] Euclidean distance, we want to use the a [23:51] more statistical notion of distance. And [23:53] in particular, very sensible one is the [23:56] notion of how many sigmas you are away [23:58] from the expectation. [24:01] It might be that you are very uncertain [24:02] in one direction and much less uncertain [24:05] in the other direction. [24:08] Like here. [24:09] So, from a statistical standpoint [24:12] this point [24:14] and this point are at the same distance [24:16] from the [24:18] from the mean. [24:19] Because loosely speaking, they are [24:22] one sigma away from the mean. [24:25] That's the notion of a Mahalanobis [24:26] distance. Basically, it measures how [24:28] many sigmas you are away from the [24:30] expectation. [24:33] Which makes sense, because if you know [24:34] that [24:35] um [24:36] you are highly uncertain along some [24:38] axis, [24:40] and then you get an observation here, [24:42] it makes sense to say that actually this [24:44] observation is indeed quite likely. [24:48] While if you get an observation [24:51] that in terms of Euclidean distance is [24:53] the same, but is this in this direction, [24:56] since you are more sigmas away from the [24:59] mean, [25:00] then you should say that this [25:01] observation is less likely. So, in a [25:03] statistical sense, this observation is [25:06] more distant from the expectation than [25:07] this observation. [25:09] Yeah. [25:10] >> Uh just to be clear, so the spaces [25:13] within the [25:15] the equal space that have equal [25:17] certainty of where you're going to be as [25:18] long as [25:19] >> Yeah, one sigma away. [25:20] >> Okay. [25:20] >> Yeah. [25:21] So, say it's a 63% probability that you [25:24] are in there. Yeah. [25:26] >> Okay. [25:30] >> Does it make sense? [25:32] So, this is referred to as Mahalanobis [25:33] distance. [25:35] Practically, how do you compute the [25:36] Mahalanobis distance? [25:38] You compute the usual [25:40] Euclidean distance, and then you scale [25:44] it by the inverse of the equal variance [25:46] matrix. [25:50] Which means that loosely speaking, if on [25:53] some axis you are very uncertain, [25:55] then the sigma is very large, and then [25:58] when you divide Euclidean distance by [25:59] the large sigma, you get a small number, [26:01] which means that statistically you are [26:02] indeed quite close. [26:06] This is an explanation from a [26:07] mathematical standpoint is extremely [26:09] brutal and in elegant, but I think it's [26:11] very practical. Do you agree? Brutal, [26:14] but effective. [26:18] Okay, so the disadvantage of using [26:20] Mahalanobis distance to do data [26:22] association [26:23] is again, [26:26] you get a measurement, you consider all [26:28] possible [26:30] um [26:31] uh associations for the correspondence [26:33] variable. [26:34] For each one of them, you compute [26:38] an expected measurement that you should [26:41] get if measurement I corresponds to [26:44] landmark K with the associated uh [26:47] um [26:50] uncertainty. [26:53] And so on. [26:55] Then, you compute the Mahalanobis [26:57] distance of the measurement that you [27:00] have observed [27:01] from [27:02] each one of the expected measurements [27:05] that you should have gotten with respect [27:07] to [27:09] a given [27:10] correspondence variable association, and [27:12] you pick the one that minimizes the [27:14] Mahalanobis distance. [27:16] This is yet another way of basically [27:18] picking picking a value for the [27:19] correspondence variable that best [27:21] explains the data. [27:30] We'll see this better later on when we [27:32] talk about EKF. [27:38] Does this make sense? Again, the idea is [27:40] to get a correspondence variable that [27:41] best explains the data. [27:43] You can do it either through maximum [27:45] likelihood estimation or by minimizing [27:47] the notion of Mahalanobis distance. [27:58] So, usually this silence it can be sign [28:00] of one or two things. [28:01] Everybody understands or everybody [28:03] doesn't understand. [28:06] >> [snorts] [28:07] >> Should I keep going? [28:10] Yeah. [28:11] >> Where is this notion of Mahalanobis [28:14] distance kind of also like maximum [28:16] likelihood estimation cuz you're picking [28:18] the point [28:18] >> Yes. Is it in a way it's very much [28:21] related. The [28:23] The philosophy is the same. [28:25] We want to keep We want to consider the [28:27] association that [28:29] best [28:31] explains what we have observed. But, [28:33] what does it mean best explain what we [28:34] have observed? In this case, best [28:37] explain means that the distance between [28:39] what we have observed and what we think [28:42] we should have observed [28:44] if [28:45] measurement I corresponding to landmark [28:47] J [28:48] this distance is as small as possible. [28:51] But, smallest [28:52] through the lens of Mahalanobis [28:54] distance, not Euclidean distance. [28:56] Such that we have a probabilistic [28:58] interpretation of the distance in terms [29:00] of a sigmas away from the mean as [29:02] opposed to just a geometric [29:03] interpretation of the distance. [29:06] Cool. So, as mentioned before, [29:09] this approach is powerful, but is quite [29:12] brittle because if you make erroneous [29:14] associations, then you keep those [29:17] through time. So, there are different [29:18] ways to deal with it. And this is a [29:20] little bit beyond the scope of class, [29:24] but one way that actually was suggested [29:26] by one of your colleagues [29:27] is to consider multi-hypothesis tracking [29:29] filter where there is a belief that is [29:32] represented as a mixture of Gaussians [29:34] and each Gaussian attracts a different [29:36] sequence of association decisions. [29:41] And of course, you don't have to use [29:42] necessarily EKF. You can also use other [29:46] parametric filters for localization such [29:48] as UKF. That is another popular choice. [29:50] But again, this is a beyond what we do [29:52] in this class. [29:54] Okay, one more item for [29:56] localization. So, here those of you that [30:00] are undergrads or taking 174 [30:02] should take a break. Well, break. Don't [30:05] step out because it's going to be just 5 [30:06] minutes. But, you're not responsible for [30:09] this material. Those of you that are [30:12] actually in the 200 version of this [30:13] class should listen to what I'm saying. [30:15] So, so far we have talked about [30:18] um [30:19] parametric approaches to localization [30:22] whereby in particular we consider [30:25] a Gaussian [30:26] as [30:28] a functional representation of the [30:29] belief as an approximation. [30:32] But, as we saw in the uh during the [30:34] discussion about filtering, another [30:37] possible approach is to consider [30:39] non-parametric approximation. [30:42] Whereby, in a way or another, we [30:44] approximate the belief through a set of [30:47] particles. [30:49] Particles, again, are concrete [30:50] instantiation [30:52] of the value of the state. [30:55] So, we saw last time how [30:57] um [30:58] this approach led to a fairly powerful [31:02] filtering algorithm, referred to as a [31:04] particle filter. [31:06] The idea, [31:08] uh [31:08] as it was the case for EKF, is to take [31:11] that [31:12] algorithm [31:13] from filtering theory, [31:16] cast it in a framework of robotic [31:18] localization, but essentially [31:20] understanding what the state transition [31:21] model in this case is, what the [31:23] measurement model in this case is, and [31:24] so on and so forth, [31:26] and then obtain the resulting [31:27] localization algorithm. [31:29] This is exactly what we're going to do [31:30] for the particle filter. In particular, [31:32] the application of the particle filter [31:34] to the problem of localization is [31:36] referred to as uh Monte Carlo [31:38] localization. [31:39] The good news is that the application of [31:42] uh [31:43] uh [31:43] the filter to the localization problem, [31:46] in the case of the particle filter, is [31:49] really straightforward. The EKF was more [31:51] complicated, [31:52] but the particle filter approach is [31:54] extremely simple. What you do is [31:56] literally you take the particle filter [31:58] that we explained last Friday, [32:00] you just substitute the state transition [32:02] model and the measurement model model [32:04] with the appropriate instantiations of [32:07] the state transition model and the [32:08] measurement models for robotics, [32:11] and that's it. [32:13] So, in particular, if you remember, the [32:15] idea of the particle filter is to [32:17] represent a belief through a set of [32:19] particles, capital M, usually about [32:21] 1,000. [32:23] And then the filter has the same [32:26] structure of a bias filter, so we have [32:28] as an input the belief at time t minus [32:30] one, current measurement, current [32:32] control, and for now as soon as we have [32:35] a map, we produce the belief at time t, [32:38] again represented as a set of m [32:39] particles. What we do, as usual, is a [32:42] prediction step where we propagate [32:45] forward the particles by using the state [32:47] transition model, and then we correct [32:49] the prediction by using information from [32:51] the measurement. The way the particle [32:53] filter does it is to propagate the [32:56] particle for [32:57] forward by using the state transition [32:59] model, [33:00] compute an importance weight [33:04] that measures how likely the measurement [33:07] that I have observed [33:09] is assuming that [33:13] the state has a value equal to the value [33:16] of that particle [33:17] a given robot. [33:20] I do this for all particles. In the [33:23] second step, what I do is to resample [33:27] the set of particles with replacement [33:30] with the probability that is [33:31] proportional with the importance weight, [33:33] whose notion, as usual, is to capture [33:36] how compatible the measurement that I [33:38] have observed [33:39] is with the hypothesis that the state is [33:42] equal to the value captured by the [33:44] particle m. [33:46] Note that it is sampling with [33:47] replacement, so it's possible that as I [33:49] resample, I always resample the same [33:51] particle. In the case where and this [33:53] happens when, for whatever reason, uh [33:57] given a given particle, [33:59] the the measurement that I have observed [34:01] is equal to has probability equal to [34:03] one, and given all the other particles, [34:06] the measurement that I have observed has [34:07] probability equal to zero. [34:09] So that I have one particle that [34:10] corresponds me to looking in that [34:12] direction, one particle that corresponds [34:14] to me to looking to that direction, and [34:16] one particle that corresponds to me to [34:17] looking in that direction, and what I [34:19] observe is a table. [34:22] So clearly, when I resample, the [34:24] probability of sampling this particle is [34:26] one. The probability of of sampling [34:28] those two particles is equal to zero [34:30] because those two particles are [34:31] incompatible with the measurement that [34:33] I've just observed. [34:34] And so I will have uh uh in the case [34:38] where we only have three particles, the [34:40] new set will be still a set with three [34:42] particles, but it will be the same [34:43] particle repeated. [34:47] And that's it. So, in hindsight, [34:51] this filter is much easier to explain, [34:54] much easier to code. Actually, the third [34:56] problem in a homework four will ask you [34:58] to code Monte Carlo localization [35:02] uh with a uh [35:04] with a tweak. [35:06] I would say I want to introduce a tweak. [35:08] The tweak is that here [35:10] the particles that is sampled in a way [35:12] that is completely independent from each [35:14] other. [35:15] And so, sometimes if you're unlucky, you [35:18] might have the issue that you get the [35:19] same particle over and over, and that [35:21] might lead it to the filter to diverge. [35:24] So, there are better ways to the [35:25] sampling by using a stochastic process, [35:28] and that's what uh you will do for [35:30] problem three, courtesy of [35:33] But it's actually interesting it it's [35:35] really simple. Uh it's an interesting uh [35:38] addition to the Monte Carlo localization [35:40] algorithm to make it remarkably more [35:42] robust. [35:43] Yep. [35:44] >> with the map into like the measurement [35:46] model and the state transition model. [35:47] Like how does that affect our our [35:49] distribution? [35:49] >> Right. So, [35:51] um [35:54] This is uh so, this something that we [35:55] discussed last time. [35:57] So, the way that we incorporate the map [35:59] in the measurement model and the state [36:00] transition model [36:02] is exactly the same regardless of [36:04] whether we use the EKF algorithm or the [36:06] particle filter algorithm. So, for [36:08] example, for the state transition model [36:10] from if you remember from last time, we [36:12] had a bit of a little bit of discussion [36:15] about what happens. So, when you [36:18] when you forward propagate the dynamics, [36:21] so you're here. [36:22] And this could be [36:24] the possible realization of your state, [36:27] maybe with more mass here at the next [36:29] time. [36:30] But now with the map, we have the [36:31] additional knowledge that we might have [36:33] obstacles. So, that should reshape this [36:36] distribution. And so, we say we say that [36:39] the way to do so is to multiply the [36:42] state transition model in the case that [36:44] we have no knowledge of the map [36:47] per times a term that [36:50] um [36:52] checks how compatible a state is with [36:54] respect to a map. It's the same business [36:56] here. So, the [snorts] discussion we had [36:57] last time ports directly to here. [37:00] So, this is an algorithm that is [37:02] relatively simple to implement [37:05] conceptually. [37:07] Quite robust, it addresses global [37:08] organization, position tracking, [37:09] kidnapping robot problems, so very [37:11] general. Can also handle dynamic [37:13] environments by doing a trial rejection [37:15] and so on. [37:16] But, there are some caveats. So, as [37:17] you'll see in your [37:20] um PSET [37:22] here we're doing a four loop. [37:25] So, if you're not careful, if you have a [37:26] large number of particles, this [37:28] algorithm becomes very slow. [37:30] So, implementation here is very [37:32] important in order to have a a real-time [37:35] particle filter. [37:37] And that's something that you'll see in [37:38] the PSET. Second of all, you might have [37:40] a more advanced versions of our sampling [37:42] the particles in order to [37:45] add robustness. [37:47] Third of all, and this is the example [37:48] here. [37:50] So, this is an application of a particle [37:51] filter. So, we have the robot that is [37:53] moving [37:54] and is getting line-of-sight [37:56] measurements. [37:57] So, the cluster of particles corresponds [37:59] to a hypothesis about where the filter [38:01] thinks the robot is. In reality, the [38:03] robot is here. [38:04] The most likely hypothesis are that the [38:06] robot is here and it is here because [38:08] based on previous measurements, this is [38:10] compatible with the both poles. As we [38:12] get inside and we start seeing a table [38:15] that is not here, these particles [38:18] disappears [38:20] when we do the resampling and finally [38:22] all the particles are concentrated [38:24] around the right poles. [38:26] So, this is the nominal operation of a [38:28] particle filter. [38:29] When you start, if you have if you know [38:31] nothing about your environment, you just [38:33] sprinkle the particles everywhere. [38:35] Remember, a particle is an hypothesis [38:37] about the concrete instantiation for [38:38] your state value. [38:40] You might get unlucky and for whatever [38:42] reason in a bad round you eliminate all [38:46] particles but one, so there is one [38:47] particle that is repeated 1,000 times. [38:50] That's a problem of particle starvation. [38:52] You don't have enough particles to [38:53] represent your belief. So, what people [38:55] do in practice is from time to time to [38:58] sprinkle some particles here and there. [39:02] Which corresponds to the notion that [39:04] your robot might have been kidnapped. [39:06] And so, it's good to have some residual [39:08] density everywhere in the environment. [39:11] Yep. [39:13] >> environment that are very similar, but [39:15] like symmetric. So, you have say the [39:17] same room above and the same room below. [39:19] So, there's no way to tell [39:21] >> Exactly. This is actually here I found [39:23] that it wouldn't work well because [39:25] that's really the case where you need to [39:26] have two modes in your belief. [39:29] If the environment is symmetric and [39:30] you're just relying on the measurements, [39:32] there is for you no way to tell whether [39:35] you are going in that direction and that [39:36] direction. So, it makes sense that you [39:38] have a belief distribution that has two [39:40] peaks. [39:41] And you keep track of both of them [39:43] simultaneously. Until hopefully you get [39:46] some measurement that allows you to [39:47] disambiguate where you are. [39:49] Now, [39:51] That's exactly what is happening here. [39:54] Exactly. So, if you see here, this [39:57] cluster, [39:58] it maps into [40:00] yeah, this cluster here. Which is [40:02] getting into here. [40:03] That's exactly what's happening here. [40:05] But that's the power of the particle [40:06] filter that allows you to keep track of [40:08] these modes. With EKF, you can't. You [40:10] have like a big Gaussian that smooths [40:13] everything. And then you see the issue [40:15] with the Gaussian EKF that one peak [40:18] should be there, one peak should be [40:20] there. With a Gaussian, the mean is [40:21] here, which is guaranteed way that [40:23] you're not there, right? [40:25] Which means that EKF should only always [40:27] be used for [40:28] local localization. So, around the path [40:32] or if you have a fairly good [40:34] initialization of your filter. If you [40:36] have no clue like in this case where we [40:37] put particles everywhere in the [40:38] environment, then the EKF is not going [40:41] to work. You have to use something that [40:42] is non-parametric. Or maybe something [40:44] that is parametric but uses mixtures of [40:46] Gaussians and so on. [40:47] >> But you also be fair to say that is [40:49] better in more sparse environments where [40:51] there's very few obstacles that would [40:53] affect your ability where you are versus [40:56] where like [40:57] MCL works better when you have very [41:00] dense environments where you can [41:03] distinguish where you are based on the [41:04] differentiation of your measurements? [41:07] >> Well, I mean, in general, the more [41:08] distinguishable landmarks you have, the [41:10] better the performance of all filters [41:12] is. So, um while it is true that uh uh [41:17] EKF localization, for example, works uh [41:20] specifically well with uh feature-based [41:22] maps, [41:23] particle filters is actually more uh uh [41:26] broadly applicable. Uh the fact of [41:28] having distinguishing landmarks is uh [41:31] uh something that's advantageous for all [41:33] filters. There's no particular [41:34] difference among them. [41:37] Yeah. [41:38] >> represents [41:39] >> What is that? Sorry. [41:40] >> What do the blue lines represent? [41:41] >> Oh, measurements. So, we're assuming [41:43] that we have uh um [41:45] a line-of-sight measurement with uh I [41:48] think uh [41:49] 10° of difference between the rays. [41:51] >> So, why are some of the walls stopping [41:53] at walls and some going through [41:55] walls? [41:56] >> Oh, yeah. I noticed uh popping is off. [42:01] Cool. All right. So, [42:05] what we're going to talk about now [42:09] is EKF SLAM. [42:13] So, we have solved the localization [42:14] problem. But, in the localization [42:16] problem, we assumed that we know the [42:17] map. [42:18] What if we do not know the map? That's [42:20] the problem of simultaneous localization [42:22] and mapping. [42:23] So, what we're going to do now is to [42:24] learn about uh in general what this SLAM [42:27] problem entails and uh how to apply [42:29] everything we know about uh parametric [42:32] filters, in particular EKF filters, to [42:34] this SLAM problem, which will lead to [42:35] the EKF SLAM algorithm. [42:38] On Friday, I was supposed to cover the [42:40] field of non-parametric SLAM algorithms, [42:42] but I'll be on travel. So, we have a [42:45] makeup lecture later on. [42:47] Non-parametric filters are not covered [42:49] by the homework, so it's not a problem [42:51] that this makeup lecture is held uh next [42:53] week or in 2 weeks from now. [42:56] Uh [42:57] readings as usual, probabilistic [42:59] robotics from Sebastian Thrun, Burgard, [43:01] and Dieter Fox, uh chapter 8 and 10. [43:06] So, in a nutshell, the SLAM problem [43:08] entails uh uh finding the [43:12] path or the pose of a robot along with a [43:14] map given the history of the [43:16] measurements from the initial time to [43:18] the current time and history of the [43:19] controls for the current time to the [43:22] uh [43:22] from the initial time to the current [43:24] time. [43:24] So, here for example, I'm showing uh [43:27] some results of [43:29] from my work about doing SLAM for [43:35] um a hopping spacecraft that is landing [43:38] on an asteroid [43:39] and has to map the asteroid as [43:44] it's uh [43:45] uh estimating its pose. [43:48] So, this asteroid usually is very small [43:50] with very low gravitational fields. [43:52] So, it takes a while to get down. The [43:54] reds are the observations that are made. [43:57] So, this is done on an asteroid uh [43:59] mock-up down at JPL with the lighting [44:02] conditions that are very representative [44:04] of what you'll have on an asteroid. So, [44:05] here you see that the robot is jumping [44:08] and in general, the SLAM algorithm does [44:11] a pretty good job in terms of uh [44:14] tracing the [44:17] trajectory. [44:19] This is not very easy for a couple of [44:21] reasons. First of all, you can see that [44:23] uh [44:24] um the lighting conditions [44:26] are either very bright or very dark. [44:30] And two, her hopping motion is uh [44:32] discontinuous. [44:34] You jump, you hit, and then you go back. [44:38] But, you can see that this can be done [44:39] actually quite reliably and in real [44:41] time. [44:42] So, that's basically SLAM. [44:44] But, this is SLAM on steroids because we [44:46] have additional issues in terms of uh [44:47] picking up uh landmarks, uh very [44:50] discontinuous motion, uh uh, large-scale [44:52] variations, and so on. [44:54] But, specifically, the SLAM problem [44:57] uh, entails [45:00] estimating, [45:01] given the history of the measurements [45:03] and the history of the controls, [45:04] the current pose and the current map. [45:07] So, before the map was a conditioning [45:09] variable, now it's a variable that we [45:11] want to estimate. So, we move the map [45:13] from the conditioning variable to the [45:15] variables that we want to estimate. [45:19] As it was the case for localization, [45:21] uh, solving the SLAM problem requires, [45:25] uh, as a primitive procedure, to be able [45:28] to associate a measurement with a [45:29] landmark. [45:30] So, if [45:32] >> [snorts] [45:32] >> that, uh, correspondence is not known, [45:34] which is typically the case, then, uh, [45:36] we also have to estimate the [45:38] correspondence variables. [45:41] And in particular, there are two [45:42] versions of the SLAM problem, online [45:44] SLAM and full SLAM. In online SLAM, we [45:47] only we're only trying to estimate the [45:49] current pose [45:50] and, uh, the map in terms of, uh, [45:53] landmarks within the map. [45:55] In, uh, full SLAM, we're trying to [45:57] estimate the entire trajectory [46:00] of the robot along with the map. [46:04] Different algorithms address, uh, [46:06] uh, one or the other. [46:08] Typically, the online SLAM leads to, uh, [46:13] sequential algorithms, like a EKF SLAM, [46:15] so it's a computationally very [46:16] tractable. But, of course, in theory, [46:18] having an estimate of the entire past [46:20] trajectory is, uh, um, [46:23] even more powerful. [46:25] In both cases, [46:27] you have the version without [46:28] correspondence variable and with [46:30] correspondence variable. Typically, you [46:32] don't have the correspondence variables [46:33] given to you, so that's a problem that [46:35] you have to solve. [46:36] Now, the reason why SLAM is complicated [46:39] is due to two main reasons. [46:41] One is that it merges, uh, optimization [46:44] over continuous variables and over, [46:47] uh, [46:48] combinatorial variables. In particular, [46:51] the positions of the landmarks, [46:55] the pose of your robot, [46:57] is a continuous quantity. Your heading [46:59] it can be anything between 0 and 2 pi. [47:01] Your position could be anything within a [47:04] given environment and so on. [47:06] The correspondence of measurement to [47:08] landmarks [47:09] is actually a combinatorial problem. [47:12] You have basically integer variables [47:14] that are assigned to each measurement [47:16] and landmark. So, it's a very hybrid [47:18] problem where some variables are [47:19] continuous, other are integer. [47:23] Second of all, [47:24] look at this probability distribution. [47:26] When we were solving the localization [47:27] problem, [47:29] uh [47:30] we were finding a posteriors with [47:32] respect to the pose. So, the map was a [47:34] conditional variable with respect to the [47:35] pose. [47:36] So, for example, if you have a unicycle [47:38] robot with three state variables x, y, [47:40] and theta, [47:42] this what we have is that the belief is [47:44] a probability distribution over how many [47:46] variables? [47:51] The belief is a probability distribution [47:53] over XT [47:54] for localization problems. [47:57] So, if XT is three-dimensional, the [47:59] belief is a probability distribution [48:01] over how many variables? [48:04] Three. Yes. Yeah, I know it's not a [48:05] trick question. [48:07] Now, we have a map, [48:10] basically a collection of landmarks. And [48:12] it typically [48:14] uh in the process of SLAM, you conduct [48:16] many, many landmarks, even like a [48:18] thousand. [48:19] Which means that now you have a [48:21] posterior over how many variables? [48:25] Thousands. [48:26] Right. So, you went from a problem where [48:28] you had a probability distribution over [48:30] two or three variables to a problem [48:32] where now you have a posterior [48:33] distribution over 1,000, 10,000 [48:35] variables. [48:37] So, okay. So, that's not very [48:38] encouraging. [48:40] So, [48:42] um [48:44] And there is another challenge, but it's [48:45] actually also an opportunity that I'm [48:47] going to discuss in a second. But before [48:49] doing so, by using our [48:52] uh beloved graphical model for the [48:54] problem, so now [48:56] in filtering, [48:58] we had states, controls, measurements [49:02] with have a map. [49:04] Uh in localization, we have a map and we [49:07] assume we know it. Now in SLAM, [49:10] we [49:12] the map influences our localization and [49:14] process, but we don't have access to the [49:16] map, so we have to estimate the map. [49:19] In online SLAM, [49:21] we estimate the current pose [49:24] >> [snorts] [49:25] >> and the map. In full SLAM, we estimate [49:28] the full thing and the map. [49:33] >> [snorts] [49:35] >> That's [49:36] the map [49:38] influences [49:41] of course the measurements in both ways, [49:43] but also the controls. For example, [49:46] because of the state transition model, [49:48] you cannot go through an obstacle. [49:54] This just to distinguish the relation [49:55] with respect to the measurement. [49:59] So another aspect of SLAM that is a [50:00] challenge but also an opportunity is as [50:02] follows. That the error with respect to [50:04] the path is correlated with with the [50:07] error with respect to the map. [50:08] What does that mean? [50:10] That [50:11] assuming that at the beginning you [50:13] there is no uncertainty about where you [50:14] are. So basically, [50:16] the [50:17] global reference system is simply [50:20] coinciding with your local reference [50:22] system. [50:24] You don't know the map, so you can put [50:26] your initial global reference system [50:27] anywhere. [50:29] The challenge is that as you move, you [50:31] want to keep that reference system [50:32] consistent. [50:33] So the uncertainty about the landmarks [50:37] is only due to measurement noise. [50:39] But now, [50:41] as you move, [50:45] you have uncertainty about where the [50:46] landmark is, which means that we have [50:48] uncertainty about where the map is. But [50:51] we have also have uncertainty about [50:52] where we are. [50:54] And that these two uncertainty are [50:55] actually related. Mean that if [50:59] So the purple ellipse is an uncertainty [51:02] ellipse. [51:03] So, if I know that I'm here, [51:06] that reduces my uncertainty about the [51:08] landmark because then I know that the [51:10] landmark most likely is around here. [51:12] If I know that I'm here, then that [51:15] reduces my uncertainty about the [51:16] landmark because then I know that the [51:17] landmark most likely is here. [51:19] Which means that by having more [51:21] information about the map, I can better [51:23] constrain information about the pose. [51:25] And by having better information about [51:26] the pose, I can better constrain [51:27] information about the map. Which means [51:29] that the two two errors are correlated. [51:32] So, it's a challenge because then to [51:34] reason about the problem correctly, we [51:35] have to reason about this correlation. [51:37] It's an opportunity because it means [51:40] that if at some point I'm able to reduce [51:44] the uncertainty about where I am, I can [51:46] also immediately reduce the uncertainty [51:48] about [51:49] the map. [51:51] And we will see that more clearly later [51:52] on. [51:56] Okay, as usual, [51:57] what we do for SLAM is to in yeah, embed [52:01] the SLAM problem as a filtering problem. [52:03] And as usual, there are two ways to [52:05] approach it. Either through parametric [52:08] approaches whereby we approximate [52:11] the belief according to a functional [52:14] relation like for example a Gaussian, [52:17] or a non-parametric approach. Basically, [52:18] we [52:19] we approximate the belief through [52:22] particle filter. [52:25] Now, [52:27] today we're going to talk about the EKF [52:29] approach to SLAM. [52:31] And we make two assumptions. So, the [52:32] first assumption is really the EKF [52:34] assumption. We assume that the motion [52:36] the motion process noise and the [52:39] measurement noise are Gaussians. [52:41] And we assume, and this is also [52:43] essential, that the map is a [52:45] feature-based. [52:47] For example, in terms of a point [52:49] landmarks. [52:51] And we have two versions of the problem [52:53] as usual. Either the correspondence [52:54] variables are known or the [52:55] correspondence variables are not known. [52:59] We first discuss the case where the [53:01] correspondence variables are known. [53:05] So, again, if you understand the [53:07] foundation of a filtering, the EKF SLAM [53:10] algorithm is not that complicated [53:12] notionally. Making it work is actually [53:14] quite complicated. But notionally it's [53:16] not that complicated. [53:20] Why? Because basically what we do is to [53:22] as we did for the localization problem, [53:24] to reduce the problem to a filtering [53:26] problem where we apply directly the EKF [53:30] result. [53:31] In particular, what is the trick of EKF [53:34] SLAM? [53:35] Is to augment [53:37] So, you remember that in SLAM we're [53:38] trying to estimate the [53:40] concurrently both the pose XT and the [53:43] map. So, the trick is to augment the [53:46] state vector. And now reason in terms of [53:49] a state vector YT [53:52] that [53:54] comprises the state of the robot. [53:57] So, YT [54:00] comprises the state of the robot XT but [54:03] also [54:05] all [54:06] uh possible landmarks. [54:08] We are in the situation where we assume [54:10] that we know the correspondence [54:12] variable, so we know how many landmark [54:13] we have. We're assuming point landmarks, [54:16] so a point landmark is represented by [54:18] the X and Y coordinate of that landmark. [54:21] So, what we do is to augment the state [54:23] vector with the X and Y coordinates [54:27] of each landmark. [54:32] We're assuming that the correspondence [54:33] variables are known which implicitly [54:35] means, since we know to to from uh which [54:38] measurement which landmark corresponds [54:40] to, [54:40] that means that we know how many [54:42] landmarks we have. [54:45] We don't know where they are, [54:46] but we know uh how many we have. [54:48] In realistic case, you don't have that [54:51] information. [54:52] And that's the case of uh EKF SLAM with [54:55] unknown correspondence that we will look [54:57] at later. [54:59] So, in that case, so if you don't know [55:01] the number of landmarks, [55:03] this is an augmented state vector that [55:05] keep growing through time. [55:07] So, it's not static but actually grows. [55:10] Okay, so the dimension of this augmented [55:13] state vector is a three. So for the case [55:15] of a unicycle we assume that the pose is [55:18] the configuration vector is [55:19] three-dimensional. If you have another [55:21] robot you might have more. [55:23] And then we assume that we have n [55:25] landmarks, two coordinates per landmark. [55:27] So the the dimension of this vector is 3 [55:30] + 2n. [55:32] Having done that, so basically the deal [55:34] of EKF SLAM is to run something that is [55:36] very similar [55:38] to EKF localization with respect to the [55:40] augmented state vector. [55:45] And in particular [55:46] we want to [55:49] Sorry, this here there is a typo. There [55:51] shouldn't be an m here because m is [55:53] already accounted for in YOT. [55:56] I will correct this. [55:58] So we want to compute the full online [56:00] posterior, so probability of YT given [56:02] the history of the measurement and [56:03] history of the controls. [56:08] Okay, so as I said, now that we have a [56:10] notion of augmented vector the idea is [56:12] literally to apply the EKF localization [56:14] with respect to this augmented vector. [56:16] As usual, in order to do so we need a [56:19] couple of things. First to specify the [56:21] transition model. [56:24] We assume we know the state transition [56:26] model for the robot. But what about the [56:28] state transition model for the augmented [56:30] vector? [56:31] The assumption that we make [56:33] is that the environment is static. We [56:36] will briefly discuss what to do in [56:37] dynamic environments. But we make the [56:39] assumption that the [56:41] environment is a static. [56:43] So what that means is that YT + 1 [56:49] or actually YT is equal to So the first [56:53] three components are equal to G [56:57] um Xt - 1 [57:00] UT. [57:02] And the other two [57:05] n components are equal to YT - 1. [57:07] So we assume that the [57:09] environment is static. [57:11] So we don't know what the landmark is, [57:13] but we know that they don't move. [57:14] So the [57:17] their coordinates that we're trying to [57:18] estimate, we know that the underlying [57:20] true values do not move. So that's why [57:22] what T is equal to what T minus one. [57:25] So this is a [57:27] three [57:29] by one vector. [57:33] Motion is affected by noise, [57:35] but we know that the landmarks do not [57:37] move. So the noise is only with respect [57:40] to the motion of the vehicle. So we have [57:43] noise with respect to the three first [57:45] three variables, but we have no noise [57:47] with respect to the other variables. [57:50] To write everything in a compact form, [57:52] we just write plus epsilon, which is [57:54] also a vector of with the three plus two [57:57] n components. [57:59] But we assume that [58:02] um [58:03] uh [58:04] RT, so the covariance, has zero entries [58:08] except for the top three by three [58:10] blocks. [58:11] Which means that we're adding zero noise [58:14] for the last two n components, meaning [58:17] this has no noise. [58:20] But we have noise for this one. [58:27] Any question about this? [58:29] So here I'm saying just something very [58:30] simple. We have the augmented vector, [58:32] two n components correspond to the [58:34] static landmarks. So the state [58:37] transition model is trivial, what T is [58:39] equal to what T minus one, what T minus [58:40] what and so on. [58:42] The first three that correspond to the [58:43] pose of the robot, three-dimensional [58:45] because we're considering a unicycle, is [58:47] affected by noise. That is [58:50] zero mean and Gaussian. [58:53] Measurement and [58:56] as as usual, this G is in general [58:58] nonlinear. We want to use EKF. So for [59:01] all the reasons that we've already [59:02] explained multiple time, we linearize [59:04] the [59:06] transition model with respect to the [59:08] most likely state. [59:10] Sorry, most likely pose in order to [59:12] avoid any confusion with the augmented [59:13] state, mu t minus one. G as usual [59:16] denotes the Jacobian. [59:18] We have to talk about the measurement [59:20] model. [59:21] Measurement model is the same as last [59:23] time. So, we we consider line of sight [59:26] range and bearing sensors. [59:28] So, given [59:30] that [59:31] uh measurement I corresponds to landmark [59:35] J, [59:37] then the range [59:39] We're assuming range and bearing sensors [59:41] as we did for EKF localization. So, the [59:43] range is the [59:46] Euclidean distance between [59:50] um [59:51] the current position of the robot and [59:53] the [59:56] position of the landmark. [59:58] So, that will be the range. So, if this [60:00] is coordinates mu J X [60:03] mu J Y [60:05] and this is X Y the position of the [60:07] robot. [60:09] So, that will be the range here. [60:11] And then the bearing [60:12] is the [60:14] um [60:16] So, if this is the theta [60:19] if this theta is the uh [60:23] this angle here phi. This is the [60:26] absolute angle minus the [60:28] uh heading theta. [60:30] Plus measurement noise. This is all what [60:32] we already saw for the case of uh [60:35] localization. [60:36] As usual, [60:38] we linearize the measurement model. [60:41] The only thing So, this is just a little [60:43] bit of an annoyance because it's a [60:44] little bit of uh [60:46] uh [60:47] notation. [60:48] But, so that So, this will be fine. We [60:51] could just have stopped it here in terms [60:52] of the measurement model. [60:53] Computationally, [60:56] when you take the [snorts] when you do [60:57] the linearization, you have to linearize [61:01] the function with respect to all the [61:03] variables Yt. [61:06] But, the measurement model assuming that [61:08] the correspondence variable are known, [61:11] depend on how many variables. So, Yt can [61:13] be very large vector. [61:16] Two uh what? 3 + 2n. [61:18] But the um [61:22] uh measurement model depends on how many [61:26] variables. [61:31] >> Five. [61:31] >> Five. X, Y, theta, mgx, mgy. [61:36] Which means that when I take derivatives [61:39] uh out of the 3 + 2n components, only [61:42] five would be non-zero. So, it would be [61:44] silly to call MATLAB or something else [61:47] to take the derivative with respect to [61:48] each variable. We already know which [61:49] ones will won't be zero. So, the idea is [61:53] to compute [61:54] a sub Jacobian, which is only the [61:57] derivative of H with respect to the [61:59] variables for which we know the Jacobian [62:01] is not zero, [62:02] and then project that back in the full [62:06] Oops, shoot. In the full Jacobian [62:09] through a projection matrix. [62:11] So, what I'm saying without going too [62:13] much into the details because it's not [62:14] that insightful. [62:16] So, here in order to project back in the [62:18] full Jacobian, so these are the [62:19] derivatives with respect to the [62:22] pose of the robot. [62:24] Which will correspond to the [62:26] top left block in the Jacobian. That's [62:28] why here we have an identity. [62:31] These are the derivatives with respect [62:33] to the j-th landmark. [62:35] So, it should only appear [62:39] uh in a very specific position of the [62:42] Jacobian. In particular, the position [62:44] that corresponds to [62:46] columns [62:49] uh [62:50] to j - 1 and 2 j. [62:53] And so, that's the other identity here. [62:54] Anything else [62:56] is a zero. [62:59] So, this is just a little bit of [63:00] bookkeeping. But, the idea here again is [63:02] that [63:04] when we take the Jacobian, only the [63:06] partial derivatives with respect to x c [63:07] and j are non-zero. [63:09] So, instead of doing something silly, [63:11] which is try to compute derivatives with [63:13] respect to all components of y t, [63:15] we only compute derivatives with respect [63:17] to x C, and U, J. And then, in order to [63:20] make the matrices correspond in terms of [63:22] dimensions, we use this big projection [63:25] matrix. [63:29] But notionally, this is just a [63:30] computational trick to avoid some silly [63:33] computations. But what we do is the [63:34] standard thing, that is to linearize our [63:36] measurement model around the most likely [63:39] value for the [63:41] state, augmented state. [63:44] Any questions here? [63:47] All right. [63:49] So, last step is how to initialize the [63:52] algorithm. [63:54] So, at the beginning, we have no clue [63:57] what the map is and where we are. [63:59] So, it's natural for [64:02] a SLAM algorithm [64:03] to take the initial pose as the origin [64:05] of the coordinate system. [64:08] So, basically, [64:10] this is where we are, [64:12] and we say that, for example, this is [64:14] the [64:15] our position is the origin of the global [64:17] coordinate system, and I just have fixed [64:20] a generic coordinate system there, and [64:22] the map that I will be constructing will [64:24] be with respect to that coordinate [64:26] system. [64:27] And then, what I want to do is to [64:28] construct a map with respect to the [64:29] coordinate system, and as the robot [64:31] moves, keep track of the transformation [64:35] between the local coordinate system of [64:36] the robot and the one that I fixed at [64:40] the beginning as my [64:42] global coordinate system. [64:44] In particular, we assume that the [64:46] at [64:48] the beginning, [64:50] the value of the state for the robot, so [64:52] the pose is zero. [64:55] So, we say that we are at the origin of [64:56] the coordinate system that we um [65:00] enforce. [65:01] And since we have no clue where all the [65:03] other landmarks are, [65:05] we set the expectation about the [65:07] position for those landmarks to zero, [65:10] and the corresponding uncertainty equal [65:12] to infinity. Sigma is the uncertainty [65:15] about [65:16] the belief for the augmented state, so [65:18] for both the pose of the [65:20] robots and the positions of the [65:21] landmarks [65:23] So [snorts] we say that uncertainty with [65:24] respect to the pose is zero. [65:26] Cuz we say that we're here. So the [65:28] uncertainty we're here is zero. [65:30] And with respect to the landmarks, we [65:31] say that uncertainty is infinite. [65:37] Any questions about this? [65:43] Cool. Very last step [65:45] as I move around, I see a landmark. [65:48] So the question is how do I initialize [65:50] the position of of that landmark? [65:55] But a natural very natural thing to do [65:57] is to take [65:59] the most likely position for your robot [66:02] as given by the [66:04] expectation in the belief of the actual [66:06] robot position of the robot [66:08] and then initialize the landmark by [66:11] using the range and bearing measurements [66:14] that I have seen and using the [66:15] measurement model. So specifically [66:23] I know that in expectation I am at uh [66:27] how do you call it? mu t x [66:31] mu t y. That's where I am. [66:34] And I received and I know that my [66:37] heading is a mu [66:40] um [66:41] t theta. [66:47] So I initialize these uh [66:50] the landmark [66:51] by and I have as a measurement r t i [66:55] f t i. [66:58] So this is my most likely expectation. [67:00] So the landmark will be somewhere along [67:03] a circle with radius r t i. More [67:06] specifically, it's given by the bearing [67:08] angle [67:09] f t i. [67:12] That's how I initialize the landmark. [67:15] It's an approximation because basically [67:16] I'm assuming that the right value of the [67:18] state is the expected value, which in [67:21] general is not true. [67:24] Does it make sense? [67:28] Cool. [67:30] Having said that, then the algorithm [67:31] looks increasingly more convoluted. But [67:34] again, I want to stress that the [67:35] intuition is always the same and most of [67:37] the mathematics is always the same. [67:40] What do we do? [67:41] We want to estimate the map and the [67:44] uh [67:45] um [67:47] pose. [67:48] So, we try to uh do that through the [67:52] biases bias filtering approach. So, as [67:54] usual, the idea is to have a a belief [67:57] that captures probability distribution [67:59] over the augmented state, not the state, [68:01] over the augmented state. [68:04] We have coming control, current uh uh [68:06] measurement, and we assume that we have [68:08] the correspondence variables. For now, [68:09] we assume that we have that information. [68:11] And we want to compute the posterior [68:12] belief accounting for the control and [68:14] the measurement. [68:15] So, we will do what we do as usual is [68:18] first step, [68:19] prediction. [68:21] We use the no- knowledge about our [68:23] motion ab- about our state transition [68:26] equation in order to predict what the at [68:29] time step t [68:31] our the value of the augmented state [68:32] vector will be. [68:35] And that here there there is no [68:37] difference with respect to EKF [68:38] localization. [68:39] What we do is to apply the state [68:42] transition model that I explained [68:43] before. [68:44] With the only difference is that the [68:46] last [68:47] two n components have trivial dynamics. [68:51] So, um [68:53] um [68:55] they the value does not change. The [68:58] uncertainty is as usual the sum of the [69:00] uncertainty due to [69:02] uh what I think I am [69:04] plus motion uncertainty. [69:09] The more complicated thing happens um [69:13] when we want to incorporate [69:14] measurements. So, what we do [69:17] is we take a measurement. [69:19] We assume that we know the [69:20] correspondence variables. [69:23] So, and then So, this measurement I then [69:25] corresponds to landmark J. [69:27] If the landmark [snorts] J has never [69:29] been seen, [69:31] I initialize it according to the [69:32] previous uh scheme. [69:34] And once I've done that, [69:37] then everything is uh almost identical [69:39] to the EKF localization algorithm. [69:42] I compute the measurement that I would [69:46] have expected [69:47] given that uh [69:49] um measurement I corresponds to landmark [69:51] J. [69:53] I compute the uncertainty in my [69:55] measurement. I blend [69:57] my um [69:59] estimate about what the state is [70:01] with the information from the [70:03] measurements, and then I update the uh [70:05] uncertainty. The main difference is that [70:08] now this view is a 3 + 2n vector. [70:13] And in particular, the common filter is [70:14] a full vector, which means that a [70:17] measurement about one landmark allow us [70:19] to refine the estimation with respect to [70:22] uh all uh landmarks, position of all [70:24] landmarks. We will see that more clearly [70:26] in a second. [70:27] >> Is that a fair statement? [70:28] Basically like if all the probabilities [70:31] are very low, so they don't actually [70:33] match up. [70:34] >> Well, we're in the um [70:36] non-correspondence case. [70:37] >> Right. [70:38] >> So, um [70:40] um [70:41] there is nothing probabilistic in say [70:44] that a J has been seen or hasn't been [70:46] seen. [70:46] >> Got it. [70:47] >> Because it's basically this function [70:48] that tells you directly. It's [70:50] unrealistic, though. [70:51] Later on, we will do that [70:52] probabilistically. That's exactly right. [70:55] So, exactly the same as EKF [70:57] localization, the main difference is [70:59] that now we have an augmented state [71:01] vector, and the dynamics for the [71:03] landmark uh [71:06] uh state variables is trivial. [71:08] Basically, we just propagate that [71:09] forward. [71:13] Okay, so how this would look like in [71:15] practice? [71:16] So, this is a robot that is moving, and [71:18] that these little blue dots are [71:20] landmarks. [71:22] Initially, I'm here. [71:24] There is uncertainty about where I am, [71:26] because basically my local coordinate [71:28] system, by definition coincides with the [71:30] global coordinate system. [71:33] I I [71:34] see this landmark and I have some [71:36] uncertainty around it due to measurement [71:39] noise. [71:40] Then I start moving. The uncertainty [71:42] about my pose increases because of [71:45] motion uncertainty. [71:47] I see another landmark and now the [71:49] uncertainty about this landmark is [71:52] relatively larger because on top of [71:54] motion noise [71:56] also on top of measurement noise I also [71:58] have uncertainty about where I am. [72:01] And then I keep going and as I get [72:03] information about additional landmarks, [72:06] since I'm increasingly uncertain about [72:09] where I am with respect to the initial [72:11] reference system, also the uncertainty [72:13] about where the landmark that I see is [72:16] is also very large. [72:19] But then here [72:21] I see the first landmark that I saw at [72:24] the beginning. [72:27] What does that mean? [72:29] That now since I saw the first landmark [72:32] that I saw at the beginning, I can [72:35] constrain myself to a very specific [72:38] location in this uncertainty ellipse. [72:43] Which [72:44] immediately reduces the uncertainty [72:46] about where the robot is. [72:48] So think it is very intuitive. So I [72:50] start [72:51] um [72:53] with seeing this chair, I move around, I [72:56] don't see this chair, so increasingly [72:58] less confident about where I am, but all [73:00] of the sudden I see this chair, I know [73:01] that I'm 1 m away from this chair. [73:04] And that constrains the estimate about [73:06] my entire past path. [73:09] Since I am 1 m away, it's not possible [73:12] that my past trajectory led me to 15 m [73:15] away just one step behind me. [73:20] Yep. [73:21] >> Is the reason that we can identify that [73:23] measurement as related to the same [73:24] feature because we have all [73:26] correspondence already? [73:27] >> Yes. [73:30] Note also how the fact that uncertainty [73:33] about the pose decreases immediately [73:36] decreases the uncertainty about the [73:38] position of all landmarks. [73:41] Why? Because if I know that I'm here, [73:44] it means [73:46] that [73:47] um [73:50] So, this landmark had this heavy [73:53] uh distribution here because I could [73:55] have been anywhere in here. [73:57] So, the landmark corresponds If I were [73:59] here, then the landmark would be here. [74:01] But since I know that I'm around here, [74:04] then it's very unlikely that the [74:05] landmark is here, so the uncertainty is [74:07] more concentrated around here, [74:08] specifically around here. So, that's [74:10] really the magic of the EKF algorithm. [74:14] That [74:14] one [74:16] um measurement that really gives con- [74:18] constraints well where you are [74:21] propagates throughout the estimator of [74:23] all the landmarks. [74:25] How that How that happens? Because the [74:28] Kalman filter is in generally fully [74:30] populated, and the variance [74:33] this sigma [74:35] that is How many dimensions does this [74:37] variance have? [74:39] How many variables we have? 3 + 2n. [74:42] So, it's 3 + 2n * 3 + 2n. [74:44] This variance is a full matrix. So, also [74:47] the off-diagonal elements are non-zero, [74:50] which means that there is correlation [74:51] among those variables. [74:52] And that's exactly what causes this [74:54] uncertainty to shrink uh simultaneously [74:57] once one of them shrinks. [75:00] Again, intuition is should be similar. [75:02] If I'm very uncertain about where I am, [75:07] then the landmark that I see, I also [75:10] have a lot of uncertainty about where it [75:11] is. But the moment I can constrain my [75:13] position, I can always also immediately [75:14] constrain the position of the landmark [75:16] that I've just seen. [75:21] Okay. [75:22] There is one additional step uh [75:26] that is what to do in the case of [75:27] unknown correspondences. I will only [75:29] give you a preview, and then we'll go [75:32] through it in more detail next time. The [75:34] preview is now that we have it to add uh [75:38] um [75:40] uh an estimation step for the [75:42] correspondence variable. [75:44] The way we do it [75:45] is by using Mahalanobis distance. [75:48] So, what we do is very similar to the [75:50] EKF localization algorithm [75:53] with unknown correspondences. Every time [75:55] I see a measurement, [75:57] I [75:58] um [76:00] assume that the measurement that I've [76:01] seen to a new landmark. [76:04] I don't know if that is the case, but [76:06] without other knowledge, I assume that [76:07] that is the case. [76:08] Then, for all possible landmarks that [76:11] I've seen, including the new one that I [76:13] postulated I've seen, [76:15] I compute the corresponding [76:17] most likely measurement that I should [76:19] have seen if measurement I corresponds [76:22] to landmark J and the corresponding [76:23] measurement uncertainty. [76:25] And then, I compute this factor pi K [76:29] that tells me how the observed [76:32] measurement is close to what I think I [76:35] should see if measurement I corresponds [76:38] to landmark J in terms of Mahalanobis [76:40] distance. [76:42] Then, [76:44] I set the pi for the [76:47] new landmark that I postulate equal to [76:49] alpha, some number that is a design [76:51] choice. [76:52] If [76:53] the minimum over all these pies is [76:56] indeed uh [76:58] n t plus one, so the new landmark, it [77:00] means that I've seen the new landmark. [77:02] So, what is this doing basically is to [77:03] create a new landmark if the Mahalanobis [77:06] distance to all existing landmarks [77:08] exceeds the value alpha. [77:12] Which which seems uh reasonable. Once [77:14] you have the assignment, everything else [77:15] is the same. [77:21] Cool. Well, um [77:23] that's it for EKF SLAM. I will just give [77:25] give you a couple more words for EKF [77:27] SLAM with unknown correspondences next [77:29] week when I'm back. [77:31] Um [77:32] but where would you take it from here? [77:34] We are not going to do it in this class, [77:36] but in more advanced class. Well, how to [77:38] make it robust? [77:40] Uh of course, [77:42] um [77:43] often times you generate especially in [77:44] the case of unknown correspondence is a [77:46] spurious landmarks. You think that the [77:48] landmark is new, but actually it's not [77:49] really new. [77:50] So, in order to robustify it, in general [77:52] you need to add some outlier rejection [77:54] schemes. [77:56] And also, so that's for the filter [77:57] itself. For the computer vision part, so [78:00] when you pick up the landmarks, try to [78:01] pick those that really are distinctive. [78:04] So, that's really the problem of feature [78:05] detection. [78:07] Detecting good landmarks is really [78:08] important. So, that's why we spent also [78:10] some time on that. [78:12] Cool. All right. So, [78:15] next week we will be talking about [78:18] sensor fusion and uh how to use open [78:21] source software for autonomy [78:23] in particular in the context of [78:25] Autoware. [78:26] But, we'll also spend that 10 more [78:28] minutes about going again through EKF [78:30] SLAM with unknown correspondences to [78:32] make sure that everyone is on the same [78:33] page. The last part was a little bit [78:35] rushed. [78:36] For the SLAM with uh [78:39] particle filter approaches, we have a [78:40] makeup lecture sometime next week or the [78:42] following week. [78:44] But, for your PSet now you have [78:45] everything you need in order to get [78:47] started with the homework four. [78:51] Cool. All right. See you next week.