Paper deep dive
Principles of Robot Autonomy
Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini, Marco Pavone
Intelligence
Status: not_run | Model: - | Prompt: - | Confidence: 0%
Entities (0)
Relation Signals (0)
No relation signals yet.
Cypher Suggestions (0)
No Cypher suggestions yet.
Abstract
Abstract:Autonomous robots are moving rapidly from research labs into everyday life - on roads, in the air, in warehouses, and in space. Robot autonomy is no longer solely an academic pursuit, but a collection of mature, field-tested methods and tools that practitioners rely on in real-world deployments. This book offers a clear, unified introduction to the methods that make this possible. Built on decades of teaching at Stanford, the text develops the core elements of modern autonomy stacks within a single conceptual framework, bridging classical robotics and modern physical AI. Every major topic is paired with hands-on Jupyter notebooks and implementation-driven exercises, so readers build practical intuition alongside theoretical understanding. The result is a principled, accessible, and deployment-aware foundation for anyone seeking to design, analyze, or contribute to the next generation of autonomous systems. This is a comprehensive resource for students, engineers, and researchers entering one of today's fastest-growing fields.
Tags
Links
- Source: https://arxiv.org/abs/2608.03496v1
- Canonical: https://arxiv.org/abs/2608.03496v1
Trouble viewing inline? Open PDF directly â
Full Text
1,208,169 characters extracted from source content.
Expand or collapse full text
Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini, Marco Pavone Principles of Robot Autonomy AUGUST 5, 2026 arXiv:2608.03496v1 [cs.RO] 4 Aug 2026 This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. Posted on arXiv with the written permission of Cambridge University Press. The definitive version will be published by Cambridge University Press. Third-party material. Figures and other material credited to external sources remain subject to the copyright of their respective owners; permission for reuse must be sought directly from those rights holders. Preface What is robot autonomy? Depending on oneâs imagination, the term robot often evokes one of two extremes. For some, it conjures visions of fantastical do-it-all android butlers from various sci-fi futures, to this day brought to life only on the page or on the screen. These helpful servants are defined by their ability to deftly handle anything the world throws at them. Endowed with human (or even super-human!) levels of ar- tificial intelligence, our aspirational creations are unbounded in creativity and resourcefulness for problem-solving. For others, the word robot refers instead to an assortment of mechatronic tools existing today, exemplified by the robotic arms and manipulators that have powered industry since the latter half of the twentieth century. This more practically-grounded interpretation is characterized by precision and control, not only in the machines themselves but also in the carefully structured environments in which they operate. Motion is optimized, and every element is arranged to occur exactly when and where it should. The production line is designed to admit no surprises, and in the rare event that there are, human supervisors intervene to restore nominal operation. In recent decades, however, a middle ground between these two extremes has begun to take shape. As illustrated in Figure 1, contemporary systemsâincluding the growing presence of self-driving cars, follow-me aerial drones, legged and humanoid robots, and free-flying space robotsâdemonstrate capabilities that lie between rigid industrial automation and fully general artificial agents. In the course of their operation, such robots inevitably encounter novel situations and unanticipated combinations of tasks and constraints. These cannot be exhaus- tively specified in advance, nor can they rely on continuous human supervision to resolve every contingency. Instead, their functionality depends on an ability to act independently in the face of uncertainty and change. In this sense, a defining feature of these systems is that they must exhibit some degree of autonomy. Robot autonomy refers to a robotâs capacity to perceive its environment and to act in pursuit of its objectives without direct external guidance, particularly from human operators. Central to this capability is the ability to make decisions based on an evolving understanding of the environment and its relation to the robotâs goals. This understanding is informed by a continual stream of sensory data, which is itself shaped by the robotâs actions as it moves through and interacts This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 2 with its surroundings. This feedback loop connecting perception and action is what enables robots to operate in unstructured and uncertain scenarios, where the ability to react and replan is paramount. (a) Waymo self-driving car.(b) NASAâs Astrobee robot.(c) Boston Dynamics Spot- Mini. (d)TeslaOptimushu- manoid robot. (e) Starship delivery robot.(f)Skydiofollow-me drone. Figure 1: Autonomous robots deployed around the world (and in the skies above!) today. Autonomy vs. Automation vs. Artificial Intelligence The notion of robot autonomy developed in this textbook overlaps with, but should not be conflated with, the broader and more established field of automa- tion, nor the rapidly evolving field of embodied artificial intelligence (AI). At a high level, what distinguishes autonomy as a subset of automation technologies is its emphasis on active decision making. More generally, automation refers to the re- duction or elimination of human involvement in the execution of tasks through a combination of different technologies. In many cases, it is advantageous to con- strain the task domain or impose additional structure on the environment so as to eliminate the need for autonomous decision-making during operation. This is not to say that automation technologies are incapable of handling variation; rather, such variations are typically anticipated during design, with predefined responses or safeguards incorporated in advance. For instance, while the exact motor commands executed by a robotic welding system may not be explicitly prescribedâsince the required levels of precision often necessitate closed-loop (feedback) controlâthe surrounding factory environment is carefully engineered in conjunction with the robot to ensure reliability and efficiency. This co-design of robot and workspace is a hallmark of classical automation. Beyond industrial manufacturing, well-established examples of automation include visual servo- ing, process control, and Computer Numerical Control (CNC) machine tools. In contrast, autonomous robotic systems are generally designed under weaker as- sumptions about environmental structure. Consequently, they must be endowed with the ability to understand their surroundings, reason, and implement novel 3 courses of action. On the other hand, the term artificial intelligence refers to the broad goal of cre- ating systems capable of perceiving, reasoning, planning, and problem-solving in ways that parallel, or exceed, human capabilities. Within robotics, AI provides many of the computational tools that enable autonomy. Classically, these have taken the form of algorithms that perform structured reasoning or search over well-defined spaces of possibilities. More recently, however, the term AI has be- come closely associated with data-driven methods, particularly machine learning (ML), in which systems acquire capabilities from data rather than through explicit programming. Embodied AI, also referred to as physical AI, extends this paradigm by grounding intelligence in a physical instantiation, or embodiment, that inter- acts with the world rather than existing solely in a virtual or abstract form. This perspective has recently been accompanied by a shift toward end-to-end system design, where the classical boundaries between perception, estimation, planning, and control are increasingly blurred or replaced by unified models. Such mod- els learn direct mappings from sensory inputs to actions, high-level decisions, or even predictions of future outcomes. Prominent examples include vision-language- action (VLA) models, which integrate multimodal input streams with action gen- eration, and emerging world models, which learn to predict the future evolution of the environment from rich sensory data such as images and video, often at internet scale. As a result, modern approaches to robot autonomy span a spec- trum, ranging from structured, model-based pipelines to fully learned, end-to- end systems. In this textbook, we present both human-designed algorithms and machine-learned approaches to provide a unified and comprehensive view of the available methodsâand because, in practice, it is often valuable to combine these approaches to various degrees depending on the application at hand. How is robot autonomy achieved? Robot autonomy is in its essence an interdisciplinary endeavor. While many ar- eas of science and engineering often benefit from the exchange of ideas across fields, robot autonomy is fundamentally defined by the integration of techniques from multiple domains. It is, by its very nature, a synthesis of computer vision, estimation theory, artificial intelligence, and control theory, just to name a few. The science and practice drawn from each of these domains form critical com- ponents of the modern autonomy stack, a term which in itself emphasizes the multifaceted, multi-component structure through which a robot perceives, rea- sons about, and interacts with its environment. At a high level, robot autonomy can be understood as requiring three fundamental capabilities: âą See: A robot uses sensors, such as cameras, laser scanners, global position- ing system measurements, and motor feedback, to collect raw data about its surroundings. These signals are processed to extract semantic and geometric information of the robotâs state and its environment. 4 Environment Sensing Perception See Filtering Localization and Mapping Decision Making Reinforcement Learning Think Trajectory Generation Motion Planning Actuator Control Act Figure 2: The See-Think-Act cy- cle. In the See stage, raw sensor signals are processed into local semantic and geometric obser- vations. In the Think stage, these observations are integrated over time to form a global estimate of the robotâs state and environ- ment, which supports higher- level decision-making.In the Act stage, decisions are trans- lated into feasible trajectories and motion plans, and executed through actuator-level feedback control. âą Think: Building on these local perceptual signals, a robot first synthesizes sen- sor data over time into a coherent global estimate of its state in the environ- ment, typically through filtering, localization, and mapping. On top of this estimate, it performs higher-level reasoning and decision-making, selecting behaviors that advance both immediate objectives and longer-horizon mission goals. âą Act: A robot executes these higher-level decisions by translating them into physically realizable motions through trajectory generation and motion plan- ning. These planned motions are then realized at the actuator level through control laws that are typically feedback-based and closed-loop, enabling robust execution despite disturbances, model mismatch, and uncertainty. The SeeâThinkâAct paradigm provides a high-level blueprint for robot auton- omy and is conceptually aligned with established human decision-making cycles such as the observeâorientâdecideâact (OODA) loop and the planâdoâcheckâact (PDCA) cycle, used in domains ranging from business to military strategy. Most modern autonomy stacks adopt some form of SeeâThinkâAct as the foundation for their behavior, although the specific implementation can vary widely de- pending on the intended application. Indeed, designing this stack is a central responsibility of the roboticist. In practice, the stages of this pipeline are often combined or treated implicitly. This is particularly evident in emerging end-to- end approaches, where the explicit boundaries between See, Think, and Act are increasingly blurred. However, this shift does not eliminate the underlying func- tional roles these components play; rather, it reflects a different way of organizing them, where, ultimately, an autonomous robot must still possess each of these ca- 5 pabilities to some degree in order to operate effectively. The steps of See-Think-Act are typically presented as a loop, where the actions executed by a robot influence the observations it receives at the next cycle. Ideally, these observations are consistent with the robotâs expectations under its current plan. Closing the loop, however, is essential for providing corrective feedback, enabling the robot to adapt to previously unseen or unexpected changes in its environment. In this way, updates to situational understanding lead to revisions of the current plan, which in turn inform the selection of the next action, ulti- mately generating new observations that initiate the cycle once again. In practice, however, this abstract loop is rarely implemented as a single, se- quential process. Instead, it is realized as a network of asynchronously operating components that run at different rates while continually incorporating the most up-to-date information available. Consider the example of a quadrotor drone. Object detection modules may process each incoming camera frame at around 30Hz and feed into object trackers operating at a similar rate, while more compu- tationally intensive tasks, such as maintaining a 3D map of the environment, may update more slowly, on the order of 2Hz. The most recent outputs from these See components can then inform a trajectory planner running at approximately 10Hz, which generates motion plans that account for both static and dynamic obstacles. At the lowest level, a flight controller tracks the planned trajectory and operates at a much higher frequency, often around 200Hz, using high-rate inertial measurements from accelerometers and gyroscopes that may reach 1000Hz. Each component in this system consumes inputs either directly from sensors or from other components, and produces outputs that are used elsewhere in the stack or ultimately translated into actuation commands. Rather than viewing auton- omy as a single closed loop, it is therefore more useful to think of the system as a graph: nodes correspond to functional components, and edges represent the flow of information between them. To make this abstraction concrete, and in the accompanying programming exercises, we will assume that communication between components is handled through the Robot Operating System (ROS), a widely used framework in both academic research and industry practice. Learning objectives This book has evolved from the course notes for the series of classes Principles of Robot Autonomy, taught annually at Stanford University since Winter 2017. The course was initiated in response to the growing deployment of self-driving cars, drones, and mobile robots more broadly, signaling a transition of robot auton- omy from a primarily academic pursuit to a collection of mature, field-tested tools and techniques on which practitioners can depend. The aim of this book is therefore to equip the reader with a principled understanding of the theoreti- cal, algorithmic, and practical aspects underlying modern robot autonomy. This is arguably an ever-moving target, particularly on the implementation side, as software tools and system abstractions continue to evolve. Nevertheless, because 6 the performance of an autonomy stack depends critically on effective system in- tegration, these practical considerations are essential and cannot be overlooked; accordingly, the Robot Autonomy Software chapter provides an orienting overview of the software principles and ROS-based implementation paradigms that under- pin modern robotic systems. At the same time, many of the fundamental principles for endowing mobile autonomous robots with perception, planning, and decision-making capabilities are now well established. The chapters that follow present the core techniques in modeling and control, motion planning and trajectory optimization, object de- tection and tracking, state estimation, simultaneous localization and mapping (SLAM), deep learning for perception and decision making, reinforcement learn- ing, imitation learning, and more. These methods are supported by mathematical tools drawn from optimization theory, geometry and coordinate transformations, filtering theory, machine learning, statistical learning theory, deep learning, and broader artificial intelligence. By developing familiarity with these foundational tools, the reader will gain a coherent understanding of the broader autonomy stack, and will be well prepared to contribute new methods and ideas to the evolving field of robot autonomy. Structure of the book Before proceeding, let us outline the structure of the book and how the material is organized. We begin with the Robot Autonomy Software chapter, which provides an overview of the software principles and tools that underpin modern robotic systems. In particular, we introduce ROS and the computational abstractions used throughout the text. While not strictly part of the autonomy stack itself, this material serves as an essential foundation for understanding how the algorithms presented in later chapters are implemented in practice. The book is then orga- nized into four parts, which reflect key aspects of the SeeâThinkâAct paradigm illustrated in Figure 2. Part I, Robot Motion Planning and Control, focuses on the Act portion of the autonomy stack, covering modeling, control, trajectory generation, and motion planning. We begin here deliberately. By grounding the discussion in how robots ultimately produce motion, we establish a concrete understanding of the final outcome of autonomy, introduce core notation and system models, and provide a pedagogical foundation upon which higher-level reasoning can be built. Part I, Robot Perception, corresponds to the See stage. It develops the tools required for extracting meaningful information from raw sensor data, including camera modeling, geometric perception, and modern learning-based approaches for detection and recognition. Part I, Robot Localization and Mapping, bridges perception and reasoning by addressing state estimation, filtering, and SLAM, forming a critical component of the Think stage. Finally, Part IV, Robot Decision Making, focuses on higher-level reasoning and 7 planning under uncertainty, including sequential decision-making, dynamic pro- gramming, reinforcement learning, and imitation learning. The book concludes with a Prospects chapter, which reflects on the broader trajectory of the field. There, we revisit the SeeâThinkâAct paradigm and discuss emerging directions. A central emphasis of this book is learning by doing. To that end, most chap- ters are accompanied by interactive Python implementations in Jupyter Note- books that allow the reader to experiment directly with the concepts and algo- rithms presented in the text. These notebooks are designed to complement the theoretical material, providing hands-on intuition and practical experience. In addition, each chapter includes exercises of varying difficulty, as well as pointers to further reading that highlight extensions, open problems, and connections to current research. Interactive notebooks and exercises are available online at: https://github.com/StanfordASL/pora-exercises.git Ultimately, each chapter is structured to include three main components: core conceptual and mathematical content, accompanying notebooks and exercises for active exploration, and references for deeper study. Together, these elements are intended to support both a principled understanding and practical proficiency in robot autonomy. We also release a regularly maintained website that includes additional teach- ing materials, updates, and resources related to the book. The official website can be found at: https://porabook.com Acknowledgments These notes accompany and are based largely on the content of the courses A174A / A274A: Principles of Robot Autonomy I and A274B: Principles of Robot Autonomy I at Stanford University. We would therefore like to acknowledge the students who have taken these courses and provided useful feedback since their initial offering in 2017. We also reserve special acknowledgements for the course assistants who were instrumental in developing and refining the course material, and in particular Benoit Landry and Edward Schmerling, who were instrumental in developing the first iteration of the courses. We would also like to acknowl- edge Yue Wang and Yan Wang for their contributions to the perception chapters, especially for developing the initial structure and organization, much of which informed and shaped the present version. We are also grateful to the members of the Autonomous Systems Lab at Stanford University for many insightful dis- cussions, careful proofreading, and helpful feedback on the material, all of which have helped shape and refine this book. Contents Preface 1 Robot Autonomy Software 15 P A R T IR O B O T M O T I O N P L A N N I N G A N D C O N T R O L 1Modeling Robot Dynamics 25 1.1State Space Models 25 1.2Kinematics and Dynamics 30 1.3Wheeled Robot Motion Models 41 1.4Simulating Robot Dynamics 45 1.5Summary 50 1.6Exercises 51 2Open-Loop Control & Trajectory Optimization 55 2.1The Optimal Control Problem 57 2.2Indirect Methods 61 2.3Direct Methods 69 2.4Differentially Flat Systems 73 2.5Summary 82 2.6Exercises 83 3Closed-Loop Control & Trajectory Tracking 89 3.1Classical Feedback Control 91 3.2Optimal Closed-loop Control for Linear Systems 96 3.3Optimal Closed-loop Control for Nonlinear Systems 104 3.4Model Predictive Control (MPC) 111 3.5Summary 117 3.6Exercises 118 4Motion Planning 127 4.1Problem Formulation 128 This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 10 4.2Grid-based Motion Planning 131 4.3Combinatorial Motion Planning 138 4.4Sampling-based Motion Planning 142 4.5Potential Field Methods 151 4.6Summary 154 4.7Exercises 155 P A R T I IR O B O T P E R C E P T I O N 5Introduction to Robot Sensors 161 5.1Sensor Classifications 161 5.2Sensor Performance 162 5.3Sensor Errors and Uncertainty Modeling 164 5.4Common Sensors in Mobile Robotics 165 5.5Summary 168 6Camera Models and Calibration 171 6.1Digital Cameras and Image Formation 171 6.2Camera Models 172 6.3RGB-D Cameras 175 6.4Perspective Projection and Coordinate Transformations 176 6.5Camera Calibration 180 6.6RGB-D Camera Calibration 188 6.7Summary 188 6.8Exercises 189 7Stereo Vision and Structure From Motion 193 7.1Stereo Vision 193 7.2Correspondence and Reconstruction 195 7.3Structure From Motion 197 7.4Summary 200 8Classical Methods for Perception 203 8.1Image Processing Fundamentals 203 8.2Geometric Feature Extraction 212 8.3Feature-Based Object Detection 219 8.4Summary 221 8.5Exercises 222 9Deep Learning Architectures for Perception 227 9.1Convolutional Neural Networks 227 9.2Transformers 231 11 9.3Point Cloud Processing and Point-Based Networks 237 9.4Voxel-Based 3D Processing 241 9.5Summary 246 9.6Exercises 247 10 Object Detection and Recognition 251 10.1 2D Object Detection Foundations 252 10.2 3D Object Detection 262 10.3Semantic and Instance Segmentation 269 10.4Summary 275 10.5Exercises 276 P A R T I I IR O B O T L O C A L I Z A T I O N A N D M A P P I N G 11 Introduction to Localization and Filtering 281 11.1Preliminary Concepts in Probability 282 11.2Markov Models 288 11.3Bayes Filter 290 11.4Summary 294 11.5Exercises 295 12 Approximate Filters for State Estimation 299 12.1The Gaussian Distribution 300 12.2Kalman Filter 302 12.3Kalman Filter Extensions 307 12.4Non-parametric Filters: From Grids to Particles 316 12.5Summary 321 12.6Exercises 323 13 Robot Localization 327 13.1A Taxonomy of Robot Localization Problems 327 13.2Robot Localization via Bayesian Filtering 330 13.3Map-Aware State Transition Model 332 13.4Markov Localization 333 13.5EKF Localization 334 13.6Monte Carlo Localization (MCL) 339 13.7Summary 340 13.8Exercises 341 14 Simultaneous Localization and Mapping (SLAM) 345 14.1SLAM Paradigms 347 14.2Front-End 349 12 14.3SLAM Across Sensing Modalities 354 14.4Mathematical Foundations of SLAM 357 14.5Extended Kalman Filter SLAM 362 14.6Particle Filter-Based SLAM 369 14.7Graph SLAM 374 14.8Factor Graph SLAM 378 14.9Advanced and Emerging Methods 383 14.10 Summary 385 14.11 Exercises 386 15 Sensor Fusion and Object Tracking 391 15.1A Taxonomy of Sensor Fusion 393 15.2Bayesian Approach to Sensor Fusion 394 15.3Engineering Realities: From Models to Working Systems 399 15.4Fusion Architectures 403 15.5Object Tracking 406 15.6Learning-Era Fusion: Features, BEV, and Cooperation 412 15.7Summary 418 15.8Exercises 419 P A R T I VR O B O T D E C I S I O N M A K I N G 16 Finite State Machines 427 16.1A Mathematical Model of Discrete Decision-Making 428 16.2Finite State Machine Architectures 430 16.3Limitations of Finite State Machines 433 16.4Summary 434 16.5Exercises 435 17 Sequential Decision Making and Dynamic Programming 437 17.1Deterministic Sequential Decision Making 437 17.2Decision Making Under Uncertainty: Markov Decision Processes 445 17.3Limitations of Dynamic Programming 450 17.4Summary 451 17.5Exercises 452 18 Reinforcement Learning 457 18.1The Reinforcement Learning Problem 457 18.2Dynamic Programming Methods 463 18.3Learning Paradigms for Model-free Control 468 13 18.4A Taxonomy of Reinforcement Learning 473 18.5Model-free Reinforcement Learning 475 18.6Model-based Reinforcement Learning 483 18.7Summary 487 18.8Exercises 487 19 Imitation Learning 495 19.1Imitation Learning in Robotics 495 19.2Behavior Cloning 498 19.3Inverse Reinforcement Learning 503 19.4Summary 508 Prospects 511 References 519 Robot Autonomy Software Modern autonomous robots are built as collections of interacting software com- ponents that perceive the environment, reason about goals, and execute actions in real time. As discussed in the Preface, autonomy is not defined by any single algo- rithm, but by the integration of many components across the SeeâThinkâAct cycle. Turning that conceptual paradigm into a functioning robotic system requires a practical software substrate through which sensing, estimation, planning, and control modules can communicate and operate together. These components must operate concurrently, exchange information reliably, and adapt to changing con- ditions, all while meeting real-world constraints on timing, safety, and robustness. Coordinating such systems is as much a software challenge as it is an algorithmic or mechanical one. In practice, most contemporary robotic systems are developed on top of a shared software infrastructure that helps manage this complexity. In this brief note, we provide a high-level overview of the Robot Operating System (ROS) 1,2 , 1 L. Joseph. Robot Operating System (ROS) for Absolute Beginners: Robotics Program- ming Made Easy. Apress, 2018 2 M. Quigley, B. Gerkey, and W. D. Smart. Programming Robots with ROS: A Practical Introduction to the Robot Operat- ing System. OâReilly Media, 2015 which has emerged as the de facto standard software framework for robotics research, education, and many industrial applications. Despite its name, ROS is not an operating system in the traditional sense. Rather, it is a middleware framework: a collection of tools, libraries, and design conventions that support modular, distributed robot software development. ROS is best understood as a common language and ecosystem for robotics software rather than a fixed or complete solution. It provides abstractions for communication, data sharing, configuration, and system integration that allow developers to focus on higher-level autonomy and control algorithms. At the same time, the robotics software landscape is evolving rapidly, and ROS itself continues to change, with new versions, extensions, and alternatives appearing regularly. As such, this chapter is not intended as a comprehensive guide or tutorial, but rather as an orienting overview of the dominant software paradigms in modern robotics. Readers are encouraged to view this material as a snapshot of current practice and to supplement it with the extensive and continually updated resources available online. This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 16 Why Robot Software Is Different At first glance, robot programming may appear to be a straightforward applica- tion of classical software engineering. However, robots impose a unique set of constraints that distinguish them from most conventional software systems. A robot must simultaneously interface with numerous hardware devices, process streaming sensor data, make decisions under uncertainty, and generate real-time control signals, all while operating in a physical world that is only partially ob- servable and constantly changing. Unlike many traditional software applications, robot software must coordinate many heterogeneous components operating concurrently. A single robot may include cameras, lidars, inertial sensors, encoders, motors, and communication interfaces, each producing or consuming data at different rates. As a result, robot software must support: âą Multitasking: autonomous robots inherently require concurrent execution. Mul- tiple sensing, estimation, planning, and control processes must run in parallel, often at different frequencies, while exchanging data in a timely and reliable manner. âą Low-level device control: robots must interface directly with a wide variety of hardware devices using communication protocols such as GPIO, USB, SPI, and others. This requires being able to support multiple programming languages, such as C++ and Python. âą High-level object-oriented abstractions: as autonomy stacks grow in size and complexity, software modularity becomes essential. Encapsulation, inheri- tance, and code reuse enable developers to build complex robotic systems from reusable components. âą Community libraries and shared infrastructure: given the breadth of robotics as a field, it is impractical for every developer to reimplement common algorithms from scratch. Access to third-party libraries and an active user community dramatically accelerates development and encourages standardization. These requirements motivate the need for a software middleware specifically designed for robotics, one that bridges the gap between hardware-level control and high-level autonomy algorithms. A Brief History of ROS Before the development of ROS, robotics software was largely fragmented. Code written for one robot or laboratory was often difficult to reuse elsewhere, hinder- ing collaboration and slowing progress across the field. In 2007, early versions of ROS emerged from the Stanford AI Robot (STAIR) project, motivated by the vision of a free and open-source robotics framework that would promote collab- oration, reuse, and rapid experimentation. 17 This vision was further realized when the startup company Willow Garage, led by Scott Hassan, assumed stewardship of the project. Under this guidance, ROS evolved into a standardized development platform featuring a rich set of tools, libraries, and conventions. The release of ROS 0.4 in 2009 coincided with the deployment of the PR2 mobile manipulation robot (Figure 3), several units of which were distributed to universities to catalyze collaborative development. ROS 1.0 followed in 2010. Figure 3: The PR2 robot, one of the first platforms to be widely supported by ROS. Oversight of ROS transitioned in 2012 to the Open Source Robotics Foundation (OSRF), supporting its development, distribution, and adoption. The first long- term support release, ROS Indigo Igloo, appeared in 2014. In 2017, OSRF was rebranded as Open Robotics, reflecting its broader mission of supporting open- source robotics software beyond ROS itself. Today, ROS is widely regarded as the closest thing to an industry standard for robot software, underpinning systems ranging from research prototypes to commercial autonomous platforms. As robotics applications expanded beyond research settings into safety-critical and real-time domains, important limitations of ROS 1 became apparent. In par- ticular, ROS 1 was not designed with real-time guarantees, deterministic com- munication, or multi-robot deployments over unreliable networks as first-class concerns. These challenges motivated the development of ROS 2, a major archi- tectural redesign that builds on modern middleware standards such as the Data Distribution Service (DDS). ROS 2 introduces native support for real-time communication, improved se- curity, better handling of distributed and multi-robot systems, and more explicit control over quality-of-service parameters. As such, ROS 2 reflects the evolv- ing needs of the robotics community and is increasingly adopted in applications where reliability, scalability, and real-time performance are critical. Design Philosophy of ROS The architectural choices embodied in ROS reflect a deliberate set of design goals shaped by the practical realities of building large-scale robotic systems. Rather than attempting to provide a monolithic, all-encompassing framework, ROS adopts a minimalist philosophy that emphasizes flexibility, reuse, and com- posability. ROSâ philosophy can be summarized in the following main principles: Peer-to-peer. At its core, ROS is designed around a peer-to-peer communication model. Computation is distributed across multiple processes, potentially running on different machines, which discover and connect to one another dynamically at runtime. This approach avoids centralized bottlenecks and allows robotic systems to scale naturally across heterogeneous computing resources, such as embedded controllers, edge devices, and cloud servers. Multi-language. Another key design principle is language neutrality. Robotic sys- tems often benefit from combining high-performance compiled code for time- 18 critical components with higher-level scripting languages for rapid development and experimentation. ROS supports this hybrid approach by defining communi- cation interfaces independently of any specific programming language, allowing components written in different languages to interoperate seamlessly. Tools-based. ROS is also explicitly tools-based. Instead of embedding all function- ality within a single runtime environment, ROS provides a collection of command- line tools and graphical utilities for tasks such as launching systems, inspecting communication graphs, logging data, replaying experiments, and visualizing in- ternal state. This modular tooling approach simplifies debugging and experi- mentation, enabling developers to inspect and modify a running system without recompilation or redeployment. Thin. Finally, ROS is intentionally thin. The middleware itself provides com- munication, configuration, and coordination mechanisms, but encourages core algorithms and device drivers to be implemented as standalone libraries with minimal dependence on ROS-specific infrastructure. This separation improves testability, promotes code reuse outside of ROS, and reduces long-term mainte- nance costs as software systems evolve. Free and open-source. ROS is released under the BSD license, which allows nearly unrestricted use, modification, and distribution, making it freely available for both academic and commercial use. As such, ROS has fostered a vibrant ecosys- tem of users and contributors who share code, documentation, and best practices. Taken together, these principles position ROS not as a rigid framework, but as an enabling substrate upon which diverse robotic architectures can be con- structed. Core ROS Concepts To reason effectively about ROS-based systems, it is helpful to understand a set of foundational abstractions that recur throughout the ecosystem. Nodes. A node is a single executable process that performs computation. Nodes are intended to be lightweight and focused, each responsible for a well-defined function. For example, a mobile robot might have separate nodes for processing camera images, fusing sensor data into a state estimate, generating motion plans, and sending commands to the motors. A complete robotic system typically con- sists of many nodes executing concurrently. Messages and Topics. Nodes communicate by exchanging messages, which are data structures defined by user-specified types. Messages are transmitted over named topics using a publisher and subscriber model, shown schematically in 19 Publisher Node A Message Publisher Node B Subscriber Node C Subscriber Node D Message Message Message Figure 4:Publish/Subscribe Communication Model in ROS. Topics are best used for unidi- rectional streaming communica- tion, where a publisher continu- ously sends messages to one or more subscribers. Sensor Interface Lidar Radar GPS & IMU Wheel Encoder Perception Localization Obstacle Detection Pose Estimation Navigation Task Planning Path Planning Vehicle Interface Steering Control Throttle/Brake Control User Interface (UI) Emergency Stop Graphical UI Global Services Vehicle Health State Data Logger Figure 5: Example structure of a mobile robot software frame- work in ROS. Each white box represents a package that con- tains multiple nodes (red boxes) responsible for specific function- ality. The arrows indicate com- munication pathways between different packages. Figure 4. A node that produces data publishes messages to a topic, while any number of other nodes may subscribe to that topic to receive the data. Publishers and subscribers are decoupled, meaning that they do not need to be aware of each otherâs existence or lifecycle, only the topic name and message type. This communication pattern naturally supports streaming data flows, such as sensor measurements, state estimates, and control commands, and enables flexible one- to-many and many-to-many information sharing. Services. In addition to asynchronous message passing, ROS also supports syn- chronous request-response interactions through services. A service defines a pair of message types, one for the request and one for the response. Nodes can adver- tise services that other nodes can call, allowing for more structured interactions such as parameter queries, configuration changes, or one-off computations. For example, a node might request an updated map, or portion of a map, from a mapping node. 20 Packages. A package is a directory that contains related ROS nodes, libraries, datasets, configuration files, and any other resources needed to build and run a particular piece of functionality. Packages are the primary unit of code organi- zation and distribution in ROS, and can be shared and reused across different projects and robots. An example of this modular structure is illustrated in Fig- ure 5. By separating functionality into well-defined components, ROS enables scalable development, testing, and maintenance of complex robotic systems. Software as the Glue of Autonomy While the algorithms presented in this book form the intellectual core of robot autonomy, their practical realization depends critically on software infrastruc- ture. In real robotic systems, autonomy is not executed as a single loop but as a distributed graph of asynchronously updating components operating at different rates and exchanging information continuously. Throughout this text, we focus on the core algorithms and their mathematical foundations, but it is important to recognize that software engineering is the glue that holds these components together in a functioning system. 21 References [58]L. Joseph. Robot Operating System (ROS) for Absolute Beginners: Robotics Programming Made Easy. Apress, 2018. [114]M. Quigley, B. Gerkey, and W. D. Smart. Programming Robots with ROS: A Practical Introduction to the Robot Operating System. OâReilly Media, 2015. Part I Robot Motion Planning and Control This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 1 Modeling Robot Dynamics Robots can take on a wide variety of forms: they may have rigid or flexible struc- tures, rely on different types of actuators for control, perceive their surroundings through diverse sensing modalities, or even exist as purely virtual agents. De- spite this variety of embodiments, nearly all robotic systems share a fundamental characteristicâthey are dynamic agents whose states evolve over time. The most immediate example of robot dynamics is physical motion, encompassing changes in position, velocity, joint configurations, and sensor orientations. A deep under- standing of the dynamical properties of a robotic system is essential to its effective design and control. For instance, building a bipedal robot capable of walking or running requires detailed modeling of its motion to ensure that the mechanical structure and actuators can withstand the forces and torques involved. Accu- rate dynamic models are also critical for designing control strategies that achieve stable, efficient, and responsive locomotion. This chapter introduces several foundational topics in modeling robotic sys- tems. We will start with Section 1.1 by introducing the concept of a state space model, which provides a mathematical framework to describe the behavior of a robotâs state over time. Next, in Section 1.2, we will detail how a robotâs kine- matics and dynamics are used to derive these state space models, focusing on the principles that govern physical motion and constraints. We will then explore spe- cific motion models for wheeled robots in Section 1.3, including the unicycle and differential drive models, to illustrate practical applications of these concepts. Fi- nally, in Section 1.4, we will discuss computational techniques for simulating robot dynamics, emphasizing numerical integration methods such as the Euler and Runge-Kutta methods to approximate and analyze the time evolution of robotic systems. 1.1 State Space Models A state space model is a mathematical framework for describing the behavior of a dynamical system. Every state space model consists of two key components: a state and a model. This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 26 principles of robot autonomy Definition 1.1.1 (State). The state of a dynamical system at time t 0 is a minimal set of variables x(t 0 ) such that, given the control input u(t) for all t â„ t 0 , the future evolution of the systemâs state x(t) for all t â„ t 0 is uniquely determined, independently of the systemâs behavior for t < t 0 . Formally, the state is a sufficient statistic of the systemâs history: given the current state and the external inputs to the system, the systemâs future behavior is fully determined, independently of how that state was reached. The state of a robotic system can be finite or infinite-dimensional. For example, a simple mo- bile robot with a rigid body can be represented by a finite and low-dimensional state (e.g., the position and velocity of its center of mass), whereas a flexible robot might require an infinite-dimensional state to describe the continuous deforma- tion of its body (e.g., a deflection function that describes the displacement at every point along the robotâs length). When modeling the dynamics of a robot, it is important to define the state in a way that aligns with the specific goals and requirements of the application. In practice, the complexity of the state representation can often be reduced by omitting parts of the robotâs dynamics that are irrelevant to the problem at hand. For instance, in developing software to enable an autonomous car to navigate urban environments, it may be sufficient to model only its position, orientation, and velocity, while abstracting away the internal mechanics of the engine, tires, or suspension. Conversely, if the car is being designed for high-performance racing, these specifics become critical to accurately capture and optimize its be- havior. Throughout this book, we focus on applications where the state is finite- dimensional and represent it as a vector xâ R n , referred to as the state vector. The second key component of a state space model is the model itself, which defines the rules and equations governing the evolution of the state over time by relating it to a set of inputs and outputs. The inputs to a model refer to external factors or control signals that influence the behavior of the system. 1 These can include forces, torques, voltages, com- 1 The term input is used interchangeably with control and action in different do- mains of robotics. mands, or any other external stimuli that drive the systemâs dynamics. As with the state, the dimensionality of the input may vary, but throughout this book we assume the input is represented by a finite-dimensional vector, uâ R m . The outputs of a model are the observable variables or measurements derived from the system. These typically come from sensors or other measurement de- vices that capture data reflecting the systemâs state and behavior. For example, a robot equipped with a global navigation satellite system (GNSS) sensor may directly measure its position but not its heading. In some cases, the relationship between the state and the output is complexâfor example, the connection be- tween a robotâs inertial pose and a red-green-blue-depth (RGB-D) camera image. The process of inferring the state from the outputs is referred to as state estima- tion, and it will be explored in detail in Chapters 11 - 15. We assume the output is finite-dimensional and denote it by the vector yâ R q . Definition 1.1.2 (Model). A model describes the evolution of a dynamical system modeling robot dynamics 27 through two types of equations: state equations and observation equations. The state equations specify how the state evolves as a function of the current state and inputs, while the observation equations define how the state influences the measurable outputs. State equations are typically modeled as differential equations 2 describing the 2 Ordinary differential equations are most commonly used to model robotic systems.However, partial differential equations may be needed for more com- plex cases, such as systems with flexible or deformable structures, where dynam- ics vary over both space and time. changes in the state with respect to an independent variable, usually time: Ìx = f (x(t), u(t)),(1.1) where Ìx = dx(t) dt is the time derivative of the state vector x(t), and f : R n Ă R m â R n is the dynamics function of the system. Observation equations take the form: y(t) = h(x(t), u(t)),(1.2) where y(t) denotes the output at time t, and h : R n Ă R m â R q is a function that relates the state and inputs to the systemâs output. Together, the definitions of the state x, input u, output y, and Equations (1.1) and (1.2), form the state space model 3 . 3 In some contexts, it may be conve- nient to represent the evolution of a sys- tem using discrete-time difference equa- tions.As we will discuss in Chap- ter 2, discrete-time models are partic- ularly well-suited for systems that nat- urally evolve in discrete steps, or for approximating continuous-time systems within computational frameworks. Throughout this book, we assume that time is the independent variable and simplify our notation by writing Ξ in place of Ξ(t) for time-dependent variables. Accordingly, we use Ì Îž to represent time derivatives, Ì Îž = d 2 Ξ(t) dt 2 for second deriva- tives, and Ξ (m) = d m Ξ(t) dt m for higher-order derivatives. In summary, state space models provide a powerful formalism for modeling, analyzing, and controlling robotic systems. Mastering and effectively utilizing state space models is crucial for advancing robotic capabilities, enabling systems to autonomously navigate, interact, and adapt in dynamic and complex environ- ments. 1.1.1 Types of State Space Models State space models, as represented in Equation (1.1) and Equation (1.2), can be classified based on two key properties: linearity and time-invariance. A model is said to be time-invariant if the functions f and h do not explicitly depend on time t; otherwise, it is time-varying. A model is said to be linear 4 if 4 Often referred to as a linear system.the functions f and h are linear functions of both the state x and control u. More generally, a system is linear if it satisfies the superposition principle, which states that the response to a linear combination of inputs is the corresponding linear combination of the individual responses. Formally, if x 1 (t) and x 2 (t) are solutions corresponding to inputs u 1 (t) and u 2 (t), respectively, then for any scalars α, ÎČ â R, the trajectory αx 1 (t) + ÎČx 2 (t) is a solution corresponding to the input αu 1 (t) + ÎČu 2 (t). If this property does not hold, the system is said to be nonlinear. Example 1.1.1 (Linear time-invariant model). Consider the system: Ì x = x + u, y = x. 28 principles of robot autonomy This model is linear and time-invariant because the functions describing the sys- tem depend linearly on x and u and there is no explicit time dependence. Example 1.1.2 (Nonlinear time-varying model). Consider the system: Ì x = tx + u, y = x 2 . This model is nonlinear due to the quadratic output term x 2 , and it is also time- varying because the state equation explicitly depends on time through the term tx. Note, however, that the state equation Ì x = tx + u is linear, as it satisfies the superposition principle with respect to x and u. Therefore, the nonlinearity in this system arises solely from the output equation, not from the state dynamics. Linear models and their standard form. Linear models are particularly important due to their analytical tractability and widespread applicability. They are com- monly expressed in a standard matrix form: Ìx = A(t)x + B(t)u, y = C(t)x + D(t)u, (1.3) where the matrices A(t), B(t), C(t), D(t) are of appropriate dimensions and de- fine the linear relationships for the state dynamics and the output. We refer to the matrix A(t) as the dynamics matrix, B(t) as the control matrix, C(t) as the output or sensor matrix, and D(t) as the direct or feed-forward matrix 5 . 5 IfthematricesA(t), B(t), C(t), and D(t) are time-invariant, we have a Linear Time-Invariant (LTI) system, which is a cornerstone of control theory. Despite the inherent nonlinear nature of many real-world systems, linear mod- els are often employed due to their simplicity and ease of manipulation. A com- mon practice is to approximate nonlinear systems with linear models around specific operating points using a technique known as linearization. As we will dis- cuss in Chapter 3, this approach simplifies analysis and control design, making linear models a fundamental tool in engineering and control theory. 1.1.2 Converting Higher-Order Models Into State Space Form The state space model, as described by Equation (1.1), is represented by first-order differential equations. However, many robotics applications involve higher-order differential equation models, such as those governing the dynamics of robotic arms or wheeled robots. To analyze and control these systems in a unified frame- work, we can convert higher-order models into first-order state space form by introducing additional state variables. Consider a linear n-th order differential equation: Ξ (n) + a nâ1 Ξ (nâ1) + . . . + a 1 Ì Îž + a 0 Ξ = u, (1.4) where a i â R are constants and Ξ is the state variable. To convert this differential equation into state space form, that is, a set of first-order differential equations, modeling robot dynamics 29 we can define an n-dimensional state vector: x = ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° x 1 x 2 . . . x n ïŁč ïŁș ïŁș ïŁș ïŁș ïŁ» : = ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° Ξ (nâ1) Ξ (nâ2) . . . Ξ ïŁč ïŁș ïŁș ïŁș ïŁș ïŁ» . Given that Ì x 1 = Ξ (n) , Ì x 2 = x 1 , Ì x 3 = x 2 and so on, we can express the higher-order differential equation as a system of first-order state space equations: ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° Ì x 1 Ì x 2 . . . Ì x n ïŁč ïŁș ïŁș ïŁș ïŁș ïŁ» = ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° âa nâ1 x 1 â . . .â a 0 x n x 1 . . . x nâ1 ïŁč ïŁș ïŁș ïŁș ïŁș ïŁ» + ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° u 0 . . . 0 ïŁč ïŁș ïŁș ïŁș ïŁș ïŁ» . In this particular case, the model is linear, so it can be represented in matrix form: Ìx = ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° âa nâ1 âa nâ2 . . . âa 1 âa 0 10. . .00 0100 . . . . . . . . . 0010 ïŁč ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁ» x(t) + ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° 1 0 . . . 0 ïŁč ïŁș ïŁș ïŁș ïŁș ïŁ» u(t). Let us consider a practical example of this technique: Example 1.1.3 (Converting Newtonâs second law to state space form). Many robotic systems, such as robotic arms and wheeled robots, involve dynamics that can be described by Newtonâs second law. Understanding how to convert these higher-order dynamics into state space form is crucial for controlling and analyz- ing numerous robotics systems. Newtonâs second law describes the acceleration response of a mass m resulting from a force F: F = m Ì s, where s represents the one-dimensional positional displacement of the mass. Newtonâs second law, in this form, is a classic example of a double integrator system 6 . 6 In control theory, a double integrator describes a system in which the output (here, the position s) is obtained by inte- grating the input (acceleration) twice. To convert this second-order differential equation into state space form, we first define the state vector: x : = " s Ì s # . Then, we represent the dynamics in state space form: Ìx = " 0 1 0 0 # x + " 0 1 m # u, where u represents the applied force F. By defining the state vector x and ex- pressing the original second-order differential equation in this manner, we have effectively transformed it into a set of first-order differential equations. 30 principles of robot autonomy 1.2 Kinematics and Dynamics Understanding a robotâs physical motion is essential for enabling its autonomous operation. For instance, it is crucial to determine how an autonomous vehicleâs actions, such as adjusting throttle and steering, affect its state and interaction with the environment. Similarly, it is important to understand how a robot manipula- torâs movements impact its ability to manipulate objects. A crucial aspect of this understanding involves the concepts of kinematics and dynamics, which govern the robotâs physical motion and the constraints it must adhere to. Definition 1.2.1 (Kinematics). Kinematics is the study of the motion of physical systems, concerned with describing positions, velocities, and accelerations over time, without reference to the forces or torques that produce the motion. A robotâs kinematics outline limitations on its motion that are determined by its physical state or geometry. These constraints arise from the physical structure of the system, such as joint limits, actuator placement, linkage geometry, and the ways in which different components are mechanically connected. They determine how the system can move, independent of any external forces. For example, consider a wheeled robot. Static friction restricts the wheels from sliding laterally, meaning they cannot move in the direction parallel to the rotation axis. This kinematic constraint significantly limits the robotâs ability to navigate and affects its overall maneuverability. By understanding these constraints, one can better grasp the feasible movements of the robot within its environment, which in turn informs how it can effectively interact with the world around it. Definition 1.2.2 (Dynamics). Dynamics is the study of the motion of physical systems as determined by the forces and torques acting upon them. It seeks to relate a systemâs motion to the underlying physical causes of that motion, such as gravity, friction, or applied inputs. In the context of robotic or mechanical systems, dynamics are typically gov- erned by Newtonâs Second Law, which states that the acceleration of a body is proportional to the net force acting on it. For example, the dynamics of an au- tonomous vehicle are described by the relationship between its acceleration and the external forces acting on it, including tire-road interaction, gravitational ef- fects on slopes, and aerodynamic drag. From the definitions above, kinematics describe constraints that arise from the robotâs physical state or geometry, whereas dynamics explain how forces or inputs influence the robotâs motion. In this section, we first introduce the concept of generalized coordinates for defining the physical configuration of a robot and discuss how the robotâs kinematics and dynamics can be expressed in terms of these coordinates. Then, we will demonstrate how to use the systemâs kinematics and dynamics to define a state space model for the robotâs physical motion. In the following chapters, we will explore how these models are applied to develop robust and high-performing algorithms for motion planning and control. modeling robot dynamics 31 Figure 1.1: Generalized coordi- nates for a wheel rolling without slipping on a plane. 1.2.1 Generalized Coordinates A robotâs physical state, also referred to as its configuration, provides a complete specification of the position of every point on the robot at a given instant 7 . A 7 T. Lozano Perez. âSpatial planning: a configuration space approachâ. In: Au- tonomous Robot Vehicles. 1990 configuration can be represented using a set of variables known as generalized coordinates, denoted by q(t)â R n g . Generalized coordinates form a set of param- eters that uniquely describe the robotâs configuration relative to a reference frame. Depending on the system, they may include joint angles, Cartesian positions, ori- entations, or other parameters defining the robotâs physical arrangement 8 . 8 The terms âconfigurationâ and âgener- alized coordinatesâ are often used inter- changeably. However, while the config- uration refers to the robotâs physical ar- rangement in space, generalized coordi- nates are a specific mathematical repre- sentation of that arrangement. Multiple choices of generalized coordinates may represent the same configuration. Importantly, the configuration, namely the generalized coordinates, typically constitutes only a part of the full system state x. As discussed in previous sec- tions, in a state-space model, the state is a complete set of variables sufficient to determine the systemâs future evolution, given an external input. This usu- ally includes both the generalized coordinates q and their time derivatives, called generalized velocities, denoted as Ìq. In some systems, the state may also include additional internal variables such as actuator dynamics, sensor states, or environ- mental parameters. Example 1.2.1 (Rolling Wheel). The configuration of a wheel rolling on a plane, as illustrated in Section 1.2.1, can be represented by three parameters: the contact point position coordinates ( x, y ) and the heading angle Ξ relative to a fixed ref- erence frame. This set of parameters, q = [ x, y, Ξ ] †, constitutes just one possible choice of generalized coordinates to define the wheelâs configuration. Alterna- tively, the configuration could also be represented using the heading angle Ξ along with a polar coordinate representation of the contact point position. 1.2.2 Kinematic Constraints Once a specific set of generalized coordinates, q, is chosen to represent a robotâs configuration, we can identify the relevant kinematic constraints for the robot. These kinematic constraints establish relationships between the generalized co- ordinates and the generalized velocities, thereby describing the limitations on the robotâs motion. We refer the reader to Siciliano et al. [132] for a comprehensive treatment of robotic kinematics and the formulation of the associated constraint equations. Definition 1.2.3 (Kinematic Constraints). Kinematic constraints are a set of con- 32 principles of robot autonomy straints imposed on the generalized coordinates, q, and generalized velocities, Ìq. We express kinematic constraints mathematically as: Ì a i (q, Ìq) = 0,i = 1, . . . , k < n g ,(1.5) where k is the number of constraints and n g is the number of generalized coordi- nates. In many robotics applications, kinematic constraints are linear with respect to the generalized velocities. These are known as Pfaffian constraints, and can be mathematically expressed as: a †i (q) Ìq = 0,i = 1, . . . , k < n g ,(1.6) where a i (q) â R n g . Pfaffian constraints can also be compactly represented in matrix form as: A †(q) Ìq = 0,(1.7) where A(q)â R n g Ăk . Figure 1.2: Generalized coordi- nates for a simple pendulum. Example 1.2.2 (Pendulum). Figure 1.2 shows a simple pendulum with a point mass and a rigid, massless, rod that rotates about a fixed pivot point. We can choose to represent the configuration of the pendulum by the Cartesian coordi- nate position of the mass, assuming the pivot point is the reference frame origin. The generalized coordinate vector for this choice is q = [ x, y ] †, and the general- ized velocity vector is Ìq = [ Ì x, Ì y ] †. The fact that the rod connecting the pivot point to the mass is rigid introduces a restriction on the motion of this system, which we represent by the kinematic constraint: Ì a 1 (q, Ìq) = x 2 + y 2 â L 2 = 0,(1.8) where L is the length of the rod. While this constraint is not in Pfaffian form, we can equivalently express it as a Pfaffian constraint by noting that: Ì a 1 (q, Ìq) = 0 =â â Ì a 1 (q, Ìq) ât = 0. For the pendulum kinematic constraint in Equation (1.8), we have: â Ì a 1 (q, Ìq) ât = 2x Ì x + 2y Ì y, and therefore we can write the constraint in the Pfaffian form of Equation (1.6) with: a †i (q) = h 2x2y i .(1.9) The Pfaffian constraint in Equation (1.9) implies that Equation (1.8) holds as long as the pendulum starts in a state q(0) satisfying Ì a 1 (q(0)) = 0. An alternative choice of generalized coordinates to represent the pendulumâs configuration is to consider the angle Ξ between the vertical and the pendulumâs rod orientation, q = [Ξ]. This choice fully specifies the configuration without requiring us to define any kinematic constraints, making it a more natural choice for this system. Note that since x = L sin Ξ and y = âL cos Ξ the kinematic constraint in Equation (1.8) is trivially satisfied for all Ξ. modeling robot dynamics 33 Example 1.2.3 (Rolling Wheel). Consider the wheel illustrated in Section 1.2.1, which we can represent with the generalized coordinates q = [ x, y, Ξ ] †. For this system, we can assume that the friction at the contact point between the wheel and the surface induces a no-slip condition. This no-slip condition is a constraint on the motion of the wheel that restricts the velocity component of the wheel in the lateral direction to always be zero. Since the unit vector e v = [ cos Ξ, sin Ξ ] †describes the heading of the wheel, the lateral direction is given by the perpen- dicular unit vector e v,â„ = [ sin Ξ, â cos Ξ ] †. We can compute the lateral velocity from the dot product of the lateral direction unit vector and the velocity vec- tor, v = [ Ì x, Ì y ] †, which gives the no-slip kinematic constraint: a 1 (q, Ìq) = Ì x sin Ξâ Ì y cos Ξ = 0.(1.10) This constraint is linear in the generalized velocities, ( Ì x, Ì y ) , and therefore is a Pfaffian constraint. 1.2.3 Holonomic and Nonholonomic Constraints Kinematic constraints often fall into two categories: holonomic or nonholonomic, depending on how they restrict the motion of the system. Holonomic constraints can be expressed solely as functions of the generalized coordinates, without in- volving generalized velocities. In contrast, nonholonomic constraints involve the generalized velocities and cannot be expressed solely in terms of the generalized coordinates. Definition 1.2.4 (Holonomic Constraints). Kinematic constraints that can be ex- pressed in the form: Ì a i (q) = 0,i = 1, . . . , k < n g ,(1.11) are called holonomic. In robotics applications, holonomic constraints generally arise due to mechan- ical interconnections, such as rigid links and joints of a robotic arm. We refer to a system that is only subject to holonomic constraints as a holonomic system. These constraints are a unique subclass of kinematic constraints that restrict the acces- sible configurations of the system. Specifically, for a system with n generalized coordinates under k holonomic constraints, the dimension of the space of acces- sible configurations is nâ k. Holonomic constraints can always be equivalently expressed as Pfaffian constraints of the form Equation (1.6). This is because: Ì a i (q) = 0 =â â Ì a i (q) ât = 0, and by differentiating the expression: â Ì a i (q) ât = â Ì a i (q) âq Ìq = a †i (q) Ìq,(1.12) as we demonstrated in Example 1.2.2. However, it is important to note that not all Pfaffian constraints are holonomic. For a Pfaffian constraint to be holonomic, 34 principles of robot autonomy it must be integrable to the form in Equation (1.11). Specifically, there must exist a scalar function Ì a i (q) such that: a †i (q) Ìq = â Ì a i (q) âq Ìq = 0.(1.13) This implies that the Pfaffian constraint, when integrated, yields a constraint solely dependent on the generalized coordinates q, without any explicit depen- dence on their time derivatives Ìq. Example 1.2.4 (Pendulum). Consider the pendulum from Example 1.2.2. The kinematic constraint in Equation (1.8) restricts the pendulum mass to lie on a circle of radius L, which is a subset of all possible generalized coordinates. This constraint is holonomic since we can express it as a function of only the general- ized coordinates. Example 1.2.5 (Rolling Wheel). Consider the wheel from Example 1.2.3, where the kinematic constraint in Equation (1.10) restricts the direction of motion. In contrast to the pendulum, this constraint does not limit the wheelâs ability to reach any configuration of generalized coordinates, namely the position and heading. Mathematically, we cannot integrate the constraint in Equation (1.10) to yield a constraint of the form Ì a i (q) = 0, and thus this constraint is not holo- nomic. While holonomic constraints restrict the systemâs accessible configurations, kinematic constraints can also limit the motion between configurations. We refer to these constraints as nonholonomic constraints. A system that is subject to at least one nonholonomic constraint is referred to as a nonholonomic system. Definition 1.2.5 (Nonholonomic Constraints). Constraints that can be described in Pfaffian form, a i (q) †Ìq = 0, but cannot be integrated to the form Ì a i (q) = 0 are called nonholonomic. That is, the Pfaffian expressions cannot be written as the total time derivative of any scalar function that depends only on the generalized coordinates. In other words, there exists no scalar function Ì a i (q) such that d dt Ì a i (q) = a i (q) †Ìq. Geomet- rically, nonholonomic constraints restrict the instantaneous generalized velocities to lie in the null space of A(q) †, where A(q) is the matrix whose rows are the vectors a i (q) †corresponding to each nonholonomic constraint. Example 1.2.6 (Rolling Wheel). Consider the wheel example from Example 1.2.3, which has a nonholonomic constraint: a 1 (q) †Ìq = h sin Ξ â cos Ξ0 i Ìq = 0. The null space of a 1 (q) †in this case is spanned by the vectors [cos Ξ, sin Ξ, 0] and [0, 0, 1], which suggests that all motion must be made up of a linear combi- nation of these vectors. Intuitively, we expect this because [cos Ξ, sin Ξ, 0] is the rolling direction and [0, 0, 1] is the axis the wheel rotates about. modeling robot dynamics 35 In summary, holonomic constraints restrict a systemâs motion by confining its configurations to lower-dimensional manifoldsâspecifically, level sets defined by scalar equations of the form Ì a i (q) = 0. For a system with n generalized co- ordinates and k independent holonomic constraints, the configuration space is effectively reduced to a manifold of dimension nâ k. This reduction reflects a true loss of accessibility in the configuration space: the system can only evolve along a restricted subset of configurations determined by the initial conditions and the constraint equations. In contrast, nonholonomic constraints act directly on the systemâs instantaneous velocities, typically expressed in Pfaffian form as A(q) †Ìq = 0, where A(q) †is a full-rank matrix of dimension kĂ n. These constraints restrict the allowed directions of motion at each configuration by con- fining Ìq to lie in an (nâ k)-dimensional subspace. However, since nonholonomic constraints are not integrable, they do not reduce the dimensionality of the con- figuration space itself. That is, although motion is constrained at each instant, the system may still be able to reach any configuration in the configuration space through admissible trajectories that respect the velocity constraints. Thus, while holonomic constraints reduce the number of independent configuration variables and confine the system to a lower-dimensional subset of the configuration space, nonholonomic constraints preserve full accessibility of the configuration space but restrict how that space can be traversed. 1.2.4 Kinematic Models Once we have chosen an appropriate set of generalized coordinates q and have identified the relevant kinematic constraints, we can convert the kinematic con- straints into a state space model of the form in Equation (1.1), which we refer to as a kinematic model. Definition 1.2.6 (Kinematic Model). Given a generalized coordinate vector q â R n g , and k Pfaffian constraints 9 , A †(q) Ìq = 0, a kinematic model is a state space 9 These Pfaffian constraints can come from a combination of holonomic and non-holonomic constraints. model of the form: Ìq = G(q)u,(1.14) where uâ R p is the input and where the column space of G(q)â R n g Ăn g âk spans the null space of A †(q). Each input in u corresponds to one degree of freedom of the system, and for any initial condition q(0) and sequence of inputs u(t) the solutions to the kinematic model are guaranteed to satisfy the Pfaffian constraints. We can prove that the trajectories of the kinematic model will satisfy the Pfaffian constraints by writing the model in the equivalent form: Ìq = G(q)u = nâk â i=1 g i (q)u i , where g i â R n g is the i-th column of G and u i is the i-th input. In this form, we can more easily see that each input acts on the generalized velocity Ìq through 36 principles of robot autonomy a particular mode that is defined by the vector g i . Since we have specified in Theorem 1.2.6 that the column space of G spans the null space of A †(q), we know that by definition: A †(q)g i (q)u i = 0, for any input u i â R and for all coordinates q. Therefore, by definition each component of the input vector can only influence the generalized velocity in a way that satisfies the Pfaffian constraints. Another way to see this mathematically is by substituting the kinematic model into the Pfaffian constraint equation: A †(q) Ìq = A †(q)G(q)u, = A †(q) nâk â i=1 g i (q)u i , = nâk â i=1 A †(q)g i (q)u i , = 0. Example 1.2.7 (Rolling Wheel). Consider the rolling wheel example from Exam- ple 1.2.3, which has a single nonholonomic constraint: a 1 (q) †Ìq = h sin Ξ â cos Ξ0 i Ìq = 0, where q = [ x, y, Ξ ] †. The null space of a 1 (q) †is spanned by the vectors [ cos Ξ, sin Ξ, 0 ] †and [ 0, 0, 1 ] †and therefore the kinematic model is given by: ïŁź ïŁŻ ïŁ° Ì x Ì y Ì Îž ïŁč ïŁș ïŁ» = ïŁź ïŁŻ ïŁ° cos Ξ0 sin Ξ0 01 ïŁč ïŁș ïŁ» " u 1 u 2 # .(1.15) In this case, the inputs u 1 and u 2 have an intuitive physical meaning: u 1 is the speed at which the wheel is moving, and u 2 is the wheelâs angular rotation rate. 1.2.5 Dynamics Models Kinematic models describe the geometric constraints that limit a robotâs motionâ for example, the fact that a robotic arm can only rotate about its joints. However, kinematics alone does not explain how motion is generated or resisted. For that, we turn to dynamics, which describe how forces and torques influence motion by producing accelerations. Newtonâs second law of motion is the foundation of robot dynamics, relating the net force acting on a body to its acceleration. Applied to a single point mass, the law takes the form: F(q, Ìq) = m Ìq,(1.16) where m is the mass of the particle, q = [ x, y, z ] †is its position vector, and F is the total force acting on the particle. modeling robot dynamics 37 Example 1.2.8 (Mass-spring-damper system). A fundamental example in the study of dynamics is the one-dimensional mass-spring-damper system. The system consists of a mass m attached to a spring and a damper, constrained to move along a line. The total force acting on the mass is typically composed of three terms: 1. an external input force F external , 2. a spring forceâkx that resists displacement from the equilibrium position, and 3. a damping forceâc Ì x that resists velocity. The net force on the mass is: F = F external â kxâ c Ì x, where k > 0 and c > 0 are the spring and damping coefficients, respectively, and x is the displacement from equilibrium. Substituting this expression into Newtonâs second law yields the following second-order differential equation: m Ì x + c Ì x + kx = F external . This equation models oscillatory motion with damping, and it arises in many robotics applicationsâfor example, when analyzing joint compliance, actuator dynamics, or contact interactions. While the mass-spring-damper system illustrates the dynamics of a single par- ticle in one dimension, real-world robotic systems are often more complex. To capture their behavior, we extend Newtonâs second law to systems of intercon- nected particles, typically modeled in robotics as rigid bodies. A rigid body is an idealized object in which the relative positions of all constituent particles remain fixed over time, regardless of external forces. This assumption implies that the body does not deform and allows us to reduce a complex system of interacting particles to a simpler model governed by the motion of a finite set of parameters, for example, position and orientation of a frame fixed to the body. As a result, rigid body dynamics provide a powerful and tractable framework for analyzing and simulating robotic systems 10 . 10 Interesting examples in robotics where the rigid body assumption may not hold include soft robots, robots with compliant end-effectors, or robots with lightweight flexible structures. The motion of a rigid body in three-dimensional space can be described in terms of its translational and rotational dynamics. Translational dynamics govern the motion of the bodyâs center of mass and are described by Newtonâs second law, as expressed in Equation (1.16), where the position variable refers specifi- cally to the center of mass. In three-dimensional space, a rigid body has three translational degrees of freedom, corresponding to movement along each of the Cartesian axes. Rotational dynamics, on the other hand, describe how the bodyâs orientation evolves over time. A rigid body also has three degrees of freedom associated with its orientation in three-dimensional space, corresponding to ro- tation about each of its principal axes. Unlike translation, orientation cannot be represented by a single vector, and several parameterizations are commonly used. 38 principles of robot autonomy Notable examples include rotation matrices, which provide a full and unambigu- ous representation at the cost of redundancy; Euler angles, which use a sequence of three rotations to represent orientation, and quaternions, which offer a compact and singularity-free alternative well-suited for numerical applications. The rotational dynamics 11 of a rigid body are governed by the time evolution 11 We refer the reader to Shuster [130] for an in-depth treatment of rotational dy- namics and attitude representations. of its angular momentum. Specifically, they are described by: M = Ì H,(1.17) where M denotes the total external moment (or torque) acting on the body, and H is the angular momentum, typically computed about the center of mass. This re- lationship is commonly referred to as Eulerâs equation for rotational dynamics and captures how applied torques influence changes in the bodyâs rotational motion. An alternative to the Newton-Euler methodâdefined by Equations (1.16)-(1.17)â for deriving the equations of motion for a rigid body is the Lagrange Method. This approach is closely tied to the notion of generalized coordinates, generalized ve- locities, and kinematic constraints, and takes an energy-based approach. In the Lagrange method, the dynamics of a rigid body are derived from a scalar quantity called the Lagrangian, defined as the difference between the kinetic and potential energies: L(q, Ìq) = T(q, Ìq)â V(q),(1.18) where T(q, Ìq) and V(q) denote the kinetic and potential energies of the system, respectively. The evolution of the system is governed by Lagrangeâs equations 12,13 , 12 B. Siciliano et al. Robotics: Modelling, Planning and Control.Springer Pub- lishing Company, Incorporated, 2008. Chap. 7 13 K. M. Lynch and K. C. Park. Modern Robotics: Mechanics, Planning, and Con- trol. Cambridge University Press, 2017. Chap. 8 which incorporate both external influences and kinematic constraints. In the ab- sence of constraints, the equations of motion are given by: d dt âL â Ìq j â âL âq j = Q j ,j = 1, . . . , n g , (1.19) where Q j â R is a non-conservative generalized force associated with the gener- alized coordinate q j 14 , and n g corresponds to the systemâs degrees of freedom. 14 Generalized forces are projections of physical forces and torques into the generalized coordinate space.Forces not derived from a potentialâsuch as frictionâare termed non-conservative. In contrast, forces like gravity are con- servative. Equations (1.19) describe how the generalized forces acting on the system relate to its position, velocity, and acceleration, providing a systematic way to derive the systemâs dynamic model from its kinetic and potential energies. In the presence of Pfaffian constraints 15 , Lagrangeâs equations take the form: 15 While Lagrangeâs method can accom- modate general constraints, we focus here on Pfaffian constraints for simplic- ity. d dt âL â Ìq j â âL âq j = Q j + k â i=1 λ i a ij (q),j = 1, . . . , n g , a †i (q) Ìq = 0,i = 1, . . . , k, (1.20) where a ij is the j-th component of the i-th Pfaffian constraint vector a i (q) and λ i â R is a Lagrange multiplier. The first n g equations describe the dynamics of the gen- eralized coordinates under the influence of both external and constraint forces, while the remaining k equations represent the kinematic constraints themselves. The complete system thus comprises n g + k equations in n g + k unknowns (the generalized coordinates and the Lagrange multipliers), and is commonly referred modeling robot dynamics 39 to as the standard non-holonomic form. If the system is holonomic and the gener- alized coordinates are chosen to be independent, the constraints are implicitly satisfied, and Lagrangeâs equations reduce to the simpler, unconstrained form introduced in Equation (1.19). Example 1.2.9 (Pendulum). Consider again the pendulum depicted in Figure 1.2. To model its dynamics, we analyze how gravity drives the motion of the mass. Specifically, we will demonstrate four distinct approaches for deriving the equa- tions of motionâusing both Cartesian and polar coordinates, and applying both the Newton-Euler and Lagrange methods. This comparison will highlight how the choice of generalized coordinates can influence the complexity of the deriva- tion. We begin by using Newtonâs second law to derive the dynamics of the pendu- lum, focusing on the two forces acting on the mass: gravity and the force from the rod. We assume that the rodâs force acts purely along its axis. Since the pendulumâs length is fixed, this force must counteract the component of gravity along the rod and generate the required centripetal acceleration. The axial force exerted by the rod is given by: F r = mg cos Ξ + mv 2 L , where m is the mass of the pendulum, g is gravitational acceleration, L is the length of the rod, and v is the speed of the mass. The gravitational force is: F g = mg, acting along the negative y-direction. To compute the net force in Cartesian co- ordinates, we project both the rodâs force and the gravitational force onto the x- and y-axes: F x =â mv 2 L sin Ξâ mg sin Ξ cos Ξ, F y = mv 2 L cos Ξâ mg sin 2 Ξ. Applying Newtonâs second law as defined in Equation (1.16) yields the equations of motion: Ì x =â v 2 L sin Ξâ g sin Ξ cos Ξ, Ì y = v 2 L cos Ξâ g sin 2 Ξ. To express these equations purely in terms of Cartesian coordinates, we substi- tute x = L sin Ξ and y =âL cos Ξ, leading to: Ì x = 1 L 2 (gxyâ xv 2 ), Ì y =â 1 L 2 (gx 2 + yv 2 ), (1.21) 40 principles of robot autonomy with v 2 = Ì x 2 + Ì y 2 . This method requires careful force analysis, as the kinematic constraint (fixed-length rod) is handled implicitly through the projected compo- nents of the rodâs force. As a second approach to deriving the equations of motion using Cartesian coordinates, we now apply the Lagrange method, which yields a slightly simpler formulation. We begin by defining the kinetic and potential energies: T = 1 2 m( Ì x 2 + Ì y 2 ),V = mgy, and observe that there are no external non-conservative generalized forces 16 . As 16 Gravity is a conservative force, so no generalized non-conservative forces ap- pear. discussed in Example 1.2.2, we recall the systemâs single Pfaffian constraint: x Ì x + y Ì y = 0. Thus, using Lagrangeâs equations introduced in Equation (1.20), we obtain: m Ì x = λx, m Ì y + mg = λy, x Ì x + y Ì y = 0. (1.22) We can solve for the Lagrange multiplier λ by differentiating the constraint with respect to time: d dt (x Ì x + y Ì y) = Ì x 2 + Ì y 2 + x Ì x + y Ì y = 0, and by substituting the expressions for Ì x and Ì y from the first two Lagrangeâs equations in (1.22) to obtain: Ì x 2 + Ì y 2 + 1 m x 2 λ + 1 m y 2 λâ gy = 0. Solving for λ yields: λ = m L 2 (gyâ v 2 ), where we used L 2 = x 2 + y 2 and v 2 = Ì x 2 + Ì y 2 . Finally, substituting the expression for λ back into the equations of motion and simplifying, we find: Ì x = 1 L 2 (gxyâ xv 2 ), Ì y =â 1 L 2 (gx 2 + yv 2 ), (1.23) which matches the result previously obtained using Newtonâs method in Equa- tion (1.21). After applying both the Newton-Euler and Lagrange methods in Cartesian co- ordinates, we now replicate the derivation in polar coordinates. To apply Eulerâs equation for rotational dynamics as given in Equation (1.17), we adopt a coordi- nate frame fixed at the pivot point. The gravitational force acting on the mass generates a moment about the pivot: M =âmgL sin Ξ, modeling robot dynamics 41 while the angular momentum of the system about the same point is: H = mL 2 Ì Îž, where mL 2 is the moment of inertia 17 about the pivot. Substituting into Eulerâs 17 In Eulerâs equation, the moment of in- ertia plays a role analogous to mass in Newtonâs second law. equation yields the system dynamics: Ì Îž =â g L sin Ξ,(1.24) which are considerably more compact than the corresponding equations derived in Cartesian coordinates. The Lagrange method also becomes significantly simpler when using the polar coordinate Ξ, as there is no need to handle Pfaffian constraints explicitly. In this formulation, the kinetic and potential energies of the system are: T = 1 2 mL 2 Ì Îž 2 ,V =âmgL cos Ξ. Applying Equation (1.20), and noting the absence of non-conservative generalized forces or constraints, we obtain the following equation of motion: Ì Îž =â g L sin Ξ, (1.25) which, as expected, matches the result derived using Eulerâs equation in Equa- tion (1.24). 1.3 Wheeled Robot Motion Models Robots are developed in diverse forms, sizes, and configurations, each featur- ing distinct mobility solutions tailored to specific applications. Among these, wheeled robots are particularly common because of their excellent mobility and simple design. In this section, we demonstrate how the concepts from the pre- ceding sections can be applied to two classic and widely used motion models for simple wheeled robots: the unicycle model and the differential drive model. 1.3.1 Unicycle Model Figure 1.3: Generalized coordi- nates for a unicycle. The unicycle model is one of the simplest kinematic models used for modeling robot motion. This model leverages the kinematics of the rolling wheel discussed in Example 1.2.3, essentially assuming the robot is constrained only by a no-slip constraint from a single wheel. Figure 1.3 illustrates a simplified diagram of the generalized coordinates for the unicycle model. The kinematic model is identical to the one presented in Equation (1.15), namely: ïŁź ïŁŻ ïŁ° Ì x Ì y Ì Îž ïŁč ïŁș ïŁ» = ïŁź ïŁŻ ïŁ° cos Ξ0 sin Ξ0 01 ïŁč ïŁș ïŁ» " v Ï # ,(1.26) 42 principles of robot autonomy where v represents the forward speed and Ï denotes the rotational rate. While the unicycle model may be a simplified representation of the robotâs true kinematics, it remains valuable in many contexts where detailed dynamics are unnecessary. Its main advantage lies in its simplicity, which often enables more computationally efficient algorithms. In practice, such lower-fidelity models are often used in the early stages of a systemâs design or decision-making process, and are later refined or supplemented with more accurate models when higher precision is required. 1.3.2 Differential Drive Model Figure 1.4:Generalized coor- dinates for a differential drive robot. The differential drive model is a variation on the unicycle model from the previ- ous section, with two wheels fixed on a shared rear axle and a passive front wheel that induces no additional kinematic constraints. This model uses the same gen- eralized coordinates as the unicycle model, q = [ x, y, Ξ ] †, but also requires the definition of certain geometric parameters: the width of the rear axle, denoted by L, and the radius of the wheels, denoted by r, as illustrated in Figure 1.4. The differential drive model assumes the wheels roll without slipping, mak- ing the derivation of its kinematic constraints similar to that of a single rolling wheel, as discussed in Example 1.2.3. The heading vector of each wheel is given by e v = [ cos Ξ, sin Ξ ] †, and the lateral direction is e v,â„ = [ sin Ξ, â cos Ξ ] †. Using the lateral direction vector, we define the no-slip kinematic constraints for the wheels as: Ìp †l e v,â„ = 0, Ìp †r e v,â„ = 0, where Ìp l and Ìp r are the velocity vectors of the left and right wheels, respectively. Next, we express the wheel velocity vectors Ì p l and Ì p r as functions of the general- ized coordinates and velocities by leveraging the robotâs geometry. The positions of the left and right wheel centers, denoted as p l and p r , respectively, can be computed from the generalized coordinates by: p l = " xâ L 2 sin Ξ y + L 2 cos Ξ # , p r = " x + L 2 sin Ξ yâ L 2 cos Ξ # . Taking the time derivative of these positions yields the velocity vectors: Ìp l = " Ì xâ Ì Îž L 2 cos Ξ Ì yâ Ì Îž L 2 sin Ξ # , Ìp r = " Ì x + Ì Îž L 2 cos Ξ Ì y + Ì Îž L 2 sin Ξ # . After some algebraic manipulation, we find that the no-slip kinematic constraints for each wheel are equivalent: Ìp †l e v,â„ = Ìp †r e v,â„ = Ì x sin Ξâ Ì y cos Ξ = 0, indicating that the no-slip constraint for both wheels is redundant, and thus the constraint matches the single wheel constraint from Example 1.2.3. This is intu- itive because the wheels are rigidly connected; hence, if one wheel cannot move modeling robot dynamics 43 laterally, neither can the other. The kinematic model for the differential drive model is also identical to the single wheel model in Equation (1.26), but the in- puts can now be expressed in a more realistic form relative to the actual geometry of the robot. In particular, instead of using the forward speed v and body rotation rate Ï as inputs, as in Equation (1.26), the differential drive model uses the rotation rates of the left and right wheels, Ï l and Ï r . We can derive a relationship between these sets of inputs by considering the geometry of the robot and the no-slip wheel assumption. First, denote the position p = [ x, y ] †in terms of the wheel center positions by p = 1 2 (p l + p r ), thus the velocity vector is Ìp = 1 2 ( Ìp l + Ìp r ). By the no-slip wheel assumption, the velocity v can be expressed as v = e †v Ì p, leading to: v = e †v Ìp, = 1 2 e †v ( Ìp l + Ìp r ), = 1 2 (v l + v r ), = r 2 (Ï l + Ï r ), where r is the radius of the wheel and v l and v r are the speeds of the left and right wheels, respectively. Additionally, the no-slip condition on each wheel is given by v l = e †v Ìp l and v r = e †v Ìp r , expanded as: Ì x cos Ξ + Ì y sin Ξâ Ì Îž L 2 = v l , Ì x cos Ξ + Ì y sin Ξ + Ì Îž L 2 = v r . Since Ì x cos Ξ + Ì y sin Ξ = v, we simplify these expressions to: L 2 Ì Îž = v r â v, L 2 Ì Îž = vâ v l . Combining these gives: L Ì Îž = v r â v l , = r(Ï r â Ï l ), establishing the relationship between the generalized velocity Ì Îž and the wheel rotational speeds. In summary, the mapping between the inputs can be defined as: v = r 2 (Ï l + Ï r ),Ï = r L (Ï r â Ï l ). which allows us to define the differential drive model: ïŁź ïŁŻ ïŁ° Ì x Ì y Ì Îž ïŁč ïŁș ïŁ» = ïŁź ïŁŻ ïŁ° r 2 cos Ξ r 2 cos Ξ r 2 sin Ξ r 2 sin Ξ r L â r L ïŁč ïŁș ïŁ» " Ï r Ï l # .(1.27) 44 principles of robot autonomy Despite the slight increase in complexity over the unicycle model, this model leverages the geometry of the robot to make the inputs more intuitive. This en- hancement makes the differential drive model more suitable for certain motion planning and control tasks, as the robotâs actuation typically originates from mo- tors attached to the wheelsâ axles. More generally, a kinematic state-space model should be interpreted only as a subsystem of a more comprehensive dynamical model. In particular, kinematic models typically assume direct control over certain motion variablesâsuch as velocity or angular rateâwithout accounting for how these quantities are gen- erated or constrained by the physical system. For more realistic modeling, it is often necessary to extend the kinematic model to include additional integrators in front of the control inputs. Example 1.3.1 (Dynamic extension of the unicycle model). The unicycle model introduced in Equation (1.26) assumes direct control over the forward velocity v and angular velocity Ï, with the state defined by the variables ( x, y, Ξ ) . To reflect the fact that velocity v is itself the result of integrating an acceleration input a, the model can be extended by treating v as an additional state, yielding the aug- mented state ( x, y, Ξ, v ) and input ( Ï, a ) . The dynamics become: ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁ° Ì x Ì y Ì Îž Ì v ïŁč ïŁș ïŁș ïŁș ïŁ» = ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁ° v cos Ξ v sin Ξ Ï a ïŁč ïŁș ïŁș ïŁș ïŁ» .(1.28) This dynamic extension accounts for acceleration as a control input and enables the modeling of more realistic scenarios, such as those involving actuation limits. 1.3.3 Bicycle/Simple Car Model The bicycle model is a simplified kinematic model commonly used to approxi- mate the motion of vehicles with two front-steered wheels and two rear-driven wheels, such as cars or mobile robots with similar geometry. The model cap- tures key steering dynamics while assuming no slip at the contact points of the wheels. It is called a âbicycleâ model because the two front wheels and two rear wheels are collapsed into a single front and rear wheel aligned on a common axis, forming a virtual two-wheeled vehicle. Compared to the unicycle and differential drive models introduced earlier, the bicycle model enforces more realistic kine- matic constraints on how the system can turn. In particular, it captures the fact that the vehicle must steer to follow curved paths, and cannot rotate in place. As such, it provides a better approximation for many wheeled systems while still remaining relatively simple. Figure 1.5 shows the simplified geometry of the bicycle model. This model can be derived by enforcing nonholonomic constraints on the rolling direction of each wheel and assuming ideal no-slip contact, following the discussion in modeling robot dynamics 45 previous sections. Figure 1.6 illustrates how the same kinematic model can be in- terpreted in the context of a four-wheeled vehicle. The key idea is that both front wheels steer with a common angle Ï, and the vehicle moves forward with veloc- ity v, subject to the no-slip constraints. These assumptions lead to the following differential equations characterizing the car model: Figure 1.5: The bicycle model approximates the motion of a four-wheeled vehicle by collaps- ing each axle into a single wheel, aligned with the vehicleâs cen- terline. The state consists of the position, ( x, y ) , of the rear axle center and the heading angle, Ξ. The control inputs are the for- ward velocity, v, and the steer- ing angle, Ï. Figure 1.6:The same bicycle model applied to a car-like vehi- cle. The control and state defini- tions are identical to Figure 1.5, but the visualization makes ex- plicit the mapping between the simplified model and a four- wheeled car. Ì x = v cos Ξ, Ì y = v sin Ξ, Ì Îž = v L tan Ï, (1.29) where ( x, y ) is the position of the rear axle center, Ξ is the heading angle, v is the forward speed, Ï is the steering angle, and L is the length of the wheelbase (the distance between the front and rear axles). Therefore, we define the state as x = [ x, y, Ξ ] †and the control input as u = [ v, Ï ] †. 1.4 Simulating Robot Dynamics In Section 1.1, we introduced the concept of a state space model to mathematically describe the evolution of a robotâs state over time. In Section 1.2 we demonstrated how a robotâs kinematics and dynamics are used to derive a state space model that represents its physical motion. In this section, we present several computational techniques for simulating the changes in a robotâs state over time. The state space model in Equation (1.1) is a general system of ordinary differ- ential equations, which in most cases cannot be solved analytically. Numerical simulation provides a practical approach to obtaining approximate solutions, al- lowing us to better understand a robotâs dynamics and to test and validate algo- rithms for robot autonomy. Typically, when we refer to simulating a system, we mean approximately solving an initial value problem (IVP) for a system of differ- ential equations, as defined in Equation (1.1): Ìx = h(x(t), t), x(t 0 ) = x 0 , where h(x(t), t) = f (x(t), u(t)), and the input u(t) may either be explicitly de- fined as a known function of time (e.g., a predefined control sequence), or com- puted at each time step based on the current state. The objective of this initial value problem is to find the trajectory x(t), starting from x(t 0 ), that satisfies the differential equation 18 . By the Fundamental Theorem 18 If h is Lipschitz continuous in x(t) and continuous in t, the trajectory x(t) exists and is unique. of Calculus 19 , the solution at time t can be written as: 19 This expresses the inverse relationship between differentiation and integration: integrating the derivative Ìx = h(x(t), t) over time recovers the original func- tion x(t). x(t) = x(t 0 ) + Z t t 0 h(x(Ï), Ï)dÏ. In general, evaluating this integral analytically for arbitrary functions h(x(t), t) is intractable. Therefore, we typically resort to numerical integration methods that 46 principles of robot autonomy involve a discretization in time: x(t) = x(t 0 ) + Nâ1 â k=0 Z t k+1 t k h(x(Ï), Ï)dÏ, (1.30) where t 0 < t 1 < · < t N = t define a time grid, and each interval has widthât k = t k+1 â t k . This decomposition breaks the continuous integration problem into a sum of smaller integrals over short intervals. Within each in- terval [t k , t k+1 ], we can then approximate the integral using various numerical quadrature rulesâsuch as the Euler method, the Midpoint method, or higher- order Runge-Kutta schemesâwhich trade off computational cost and accuracy. In this section, we provide a concise introduction to some of the most widely used methods. Example 1.4.1 (Simple IVP). To illustrate the different numerical integration meth- ods, we will consider the initial value problem defined below and presented in Algorithm 1 as an example: Ì x(t) = x(t) sin 2 (t),x(0) = 1. Our goal is to approximate the trajectory x(t) over the interval [0, 10] using vari- ous integration methods. Concretely, we will explore different techniques to approximate the following analytical solution: x(t) = x 0 exp tâ t 0 â sin(tâ t 0 ) cos(t + t 0 ) 2 , which, for our specific initial conditions x 0 = 1 and t 0 = 0, simplifies to: x(t) = exp tâ sin(t) cos(t) 2 . 1.4.1 Euler Method One of the simplest techniques for approximating the integral within each time interval of the discretized problem, as shown in Equation 1.30, is the Euler method 20 . This method approximates the integral over a short interval [t k , t k+1 ] 20 Named after the Swiss mathematician Leonhard Euler and often referred to as the forward Euler method. by evaluating the integrand at the beginning of the interval. Given that the trajectory x(t) satisfies: x(t k+1 ) = x(t k ) + Z t k+1 t k h(x(Ï), Ï)dÏ, the Euler method approximates this integral by assuming h(x(Ï), Ï) is constant within the interval, yielding: x(t k+1 )â x(t k ) +ât· h(x(t k ), t k ), whereât = t k+1 â t k is the time step. modeling robot dynamics 47 Simple IVP (Running Example) import numpy as np # Define the derivative function h(x, t) def h(x, t): return x * np.sin(t) ** 2 # Define initial conditions and final time x0 = 1.0 # Initial state x(t0) t0 = 0.0 # Initial time t0 tf = 10.0 # Final time tf ât = 0.5 # Discretization step t = np.arange(t0, tf +ât,ât) # Array of timestamps # Compute analytical solution x _ true = x0 * np.exp(((t-t0) - np.sin(t-t0) * np.cos(t+t0))/2) def integrate(h, x0, t, method): x = np.zeros((t.size, x0.size)) x[0] = x0 for i in range(t.size - 1): ât = t[i + 1] - t[i] x[i + 1] = method(h, x[i], t[i],ât) return x def method(h, x, t,ât): # Implement here numerical integration method # Test a specific numerical method x _ method = integrate(h, x0, t, method) Algorithm 1: Definition of an illustrative initial value problem. The code for this example is available in the repository github.com/StanfordASL/pora-exercises in the notebook ch01/simulation.ipynb. In the following sections, we will explore different numerical integration methods and implement them in a custom method function. This approximation corresponds to a first-order Taylor series expansion: x(t +ât)â x(t) +ât Ìx(t), = x(t) +ât· h(x(t), t), (1.31) which treats the rate of change Ìx(t) as constant across the interval. 48 principles of robot autonomy Alternatively, the Euler method can be interpreted as a finite difference ap- proximation of the time derivative: Ìx(t)â x(t +ât)â x(t) ât . While computationally inexpensive, the Euler method has limited accuracy due to its reliance on information from the beginning of each interval. The local truncation error 21 is of order O(ât 2 ), and errors can accumulate significantly 21 That is, the error introduced in a single time step. over long trajectories unless small time steps are used. As a concrete illustration, consider the running example introduced in Ex- ample 1.4.1. A simple implementation of Eulerâs method is presented in Algo- rithm 2. Euler Method def euler(h, x, t,ât): return x +ât * h(x, t) x _ euler = integrate(h, x0, t, euler) Algorithm 2: Python implementation of Eulerâs method. 1.4.2 Midpoint Method The Midpoint method is a refinement of the Euler method that improves accuracy by evaluating the derivative at the midpoint of the time interval, rather than at its beginning. Recall that Eulerâs method approximates the next state using the derivative Ìx evaluated at time t: x(t +ât)â x(t) +ât· h(x(t), t). In contrast, the Midpoint method approximates the integral by using the value of the derivative at t + ât 2 : x(t +ât)â x(t) +ât· h x t + ât 2 , t + ât 2 .(1.32) While this yields a more accurate estimate, it is not yet explicit, since the value x(t + ât 2 ) is not known in advance. To resolve this, we approximate the midpoint using a single Euler step of size ât 2 : x t + ât 2 â x(t) + ât 2 · h(x(t), t).(1.33) Substituting this estimate into Equation (1.32), we arrive at the explicit form of the Midpoint method: x(t +ât)â x(t) +ât· h x(t) + ât 2 · h(x(t), t), t + ât 2 . modeling robot dynamics 49 The Midpoint method improves the local truncation error with respect to the Euler method fromO(ât 2 ) toO(ât 3 ), providing significantly better accuracy for small step sizes. The improvement comes at the cost of computing the derivative twice per stepâonce at the start of the interval and once at its midpoint. As an illustration, a simple implementation for the running example intro- duced in Example 1.4.1 is presented in Algorithm 3. Midpoint Method def midpoint(h, x, t,ât): t _ mid = t +ât / 2 x _ mid = x + (ât / 2) * h(x, t) return x +ât * h(x _ mid, t _ mid) x _ midpoint = integrate(h, x0, t, midpoint) Algorithm 3: Python implementation of the Midpoint method. 1.4.3 Runge-Kutta-4 Method Both the Euler and Midpoint methods approximate the change in x over a time step interval by evaluating the derivative Ìx at one or two specific points within the interval. The Runge-Kutta family of methods generalizes this idea by using multiple evaluations of the derivative to achieve higher accuracy. One of the most widely used methods in this family is the fourth-order Runge-Kutta method 22 , 22 Often abbreviated as RK4.which computes four derivative estimates over the interval [t, t +ât]: x(t +ât)â x(t) + ât 6 (k 1 + 2k 2 + 2k 3 + k 4 ), where: k 1 = h(x(t), t), k 2 = h(x(t) + ât 2 k 1 , t + ât 2 ), k 3 = h(x(t) + ât 2 k 2 , t + ât 2 ), k 4 = h(x(t) +âtk 3 , t +ât). RK4 improves the local truncation error toO(ât 5 ), offering significantly better accuracy than the Euler or Midpoint methods. This comes at the cost of evaluat- ing h(x, t) four times per step, but the method remains computationally efficient and stable for many practical applications. Using the running example from Example 1.4.1, an implementation of RK4 is shown in Algorithm 4. 50 principles of robot autonomy RK4 Method def rk4(h, x, t,ât): k1 = h(x, t) k2 = h(x + (ât / 2) * k1, t +ât / 2) k3 = h(x + (ât / 2) * k2, t +ât / 2) k4 = h(x +ât * k3, t +ât) return x + (ât / 6) * (k1 + 2 * k2 + 2 * k3 + k4) x _ rk4 = integrate(h, x0, t, rk4) Algorithm 4: Python implementation of the RK4 method. 0246810 t 0 50 100 x ( t ) Analytical Solution RK4 Method Midpoint Method Euler Method Figure 1.7: Visual comparison of various numerical integration methods and their approxima- tions compared to the analyti- cal solution for the initial value problem introduced in Exam- ple 1.4.1, withât = 0.5. To compare the performance of these methods, we can visualize their outputs against the analytical solution: It is important to highlight that advanced simulation techniques extend far beyond these numerical integration methods, allowing for the high-fidelity simu- lation of robotic systems, including visualization in pixel space. These techniques can incorporate detailed physical modeling, sensor data fusion, and learning- based approaches to create realistic and accurate simulations 23 . As we will dis- 23 One notable example is the use of Neural Radiance Fields (NeRFs) for gen- erating photorealistic scenes. cuss in later chapters on perception, these advancements are crucial for tasks such as robot training, planning under uncertainty, and evaluating the autonomy stack in novel and previously unseen scenarios. 1.5 Summary In this chapter, we introduced the fundamental principles underlying the mod- eling and simulation of robotic systems. We began by introducing state space models, which provide a mathematical framework to describe the evolution of a robotâs state over time. Next, we discussed a robotâs kinematics and dynamics, which characterize its motion and the constraints acting on it. This included a discussion of generalized coordinates and kinematic constraintsâboth holonomic modeling robot dynamics 51 and nonholonomicâalong with the formulation of kinematic models using Pfaf- fian constraints. Throughout this chapter, we examined practical examples such as the rolling wheel, the pendulum, and wheeled robots like the unicycle and dif- ferential drive models. Finally, we introduced numerical integration techniques for simulating robot dynamics over time. We presented the Euler, Midpoint, and Runge-Kutta methods, highlighting their trade-offs and applications through con- crete code examples. To learn more. For readers interested in a deeper and more rigorous treatment of the concepts presented in this chapter, Siciliano and Khatib [131] and Sicil- iano et al. [134] offer comprehensive and widely adopted references. These texts cover the mathematical foundations of robot kinematics, dynamics, and control in greater depth, and provide additional examples, derivations, and exercises that complement and extend the material introduced here. 1.6 Exercises The starter code for the exercises provided below is available online through GitHub. To get started, download the code by running in a terminal window: git clone https://github.com/StanfordASL/pora-exercises.git We denote Problems requiring hand-written solutions and coding in Python withand, respectively. Problem 1: Numerical Integration Methods In Example 1.4.1 we introduced a simple IVP and in Section 1.4.3 we showed the differences between the Euler, midpoint, and fourth-order Runge-Kutta methods for solving it. In this exercise we will explore the use of a more advanced numeri- cal integration scheme provided by the SciPy Python library called odeint. Open the notebook ch01/exercises/simulation.ipynb and practice by implementing the dynamics models for a damped pendulum and a bicycle, and then simulating them using odeint. Problem 2: Nonholonomic Wheeled Robot Dynamics The goal of this exercise is to familiarize yourself with some Python fundamentals that will be used throughout the book, such as NumPy and inheritance, as well as techniques for controlling nonholonomic wheeled robots. Consider a simple robot with two wheels whose state is defined by the position of the center of the axle and the heading angle, shown in Figure 1.8. This robotâs motion can be described by the simplest nonholonomic wheeled robot model, the unicycle model. Figure 1.8: Generalized coordi- nates for a robot with unicycle kinematics. 52 principles of robot autonomy The kinematic model we will use reflects the rolling without side-slip constraint, and is given below in Equation (1.34). Ì x(t) = v(t) cos(Ξ(t)), Ì y(t) = v(t) sin(Ξ(t)), Ì Îž(t) = Ï(t). (1.34) In this model, the robot state is x = [x, y, Ξ] †, where [x, y] †is the Cartesian location of the robot center and Ξ is its heading with respect to the x-axis. The robot control inputs are u = [v, Ï] †, where v is the velocity along the main axis of the robot and Ï is the angular velocity, subject to the control constraints: |v(t)|†0.75 m/s,and |Ï(t)|†1.0 rad/s. In this problem, we will demonstrate the use of class inheritance in Python classes and the use of NumPy for vectorized operations. The notebook associated with this problem is ch01/exercises/nonholonomic _ wheeled _ robot _ dynamics.i pynb. We will be using a Dynamics base class for two different dynamics models: the wheeled robot dynamics model in this exercise and a double integrator model in the next exercise. The base class contains two unimplemented functions: step and rollout. The step function will propagate the dynamics a single time step with disturbances, and the rollout function will apply the step function multiple times to retrieve a trajectory of states over multiple time steps. Because the feed- forward dynamics are subject to disturbances, the same control sequence will result in different trajectories. We will observe this by executing multiple rollouts of the dynamics using the same control sequence from the same initial state. In the ch01/exercises/nonholonomic _ wheeled _ robot _ dynamics.ipynb note- book, complete the RobotDynamics class. Implement the function step using discrete-time Euler integration with the kinematic equations described in Equa- tion (1.34). Then in the same class, implement the function rollout with two for-loops, calling the step function. Run the cells that rollout the robotâs dy- namics and plot the control and state trajectories (this code has been written for you). Problem 3: Double Integrator Dynamics In this exercise, we consider the double integrator dynamics model: Ì x(t) = v x (t), Ì y(t) = v y (t), Ì v x (t) = a x (t), Ì v y (t) = a y (t). (1.35) In this model, the robot state is x = [x, y, v x , v y ] †and the robot control inputs are u = [a x , a y ] †. modeling robot dynamics 53 Notice that in the previous problem, we used a for-loop to rollout several trajectories of the robotâs dynamics. In this problem, we will use the same base dynamics class for a DoubleIntegratorDynamics class, and use batching to reduce the number of for-loops needed to perform multiple rollouts. The notebook for this problem is ch01/exercises/double _ integrator _ dynamics.ipynb. i. To reduce the number of for-loops needed to perform multiple rollouts, we will batch the dynamics equations applied in the function step. Implement the function step in the DoubleIntegratorDynamics class. i. Fill in the code in function rollout in the DoubleIntegratorDynamics class using the step function you just wrote. Note that you should only need one for-loop! 54 principles of robot autonomy References [88]T. Lozano Perez. âSpatial planning: a configuration space approachâ. In: Autonomous Robot Vehicles. 1990. [90]K. M. Lynch and K. C. Park. Modern Robotics: Mechanics, Planning, and Control. Cambridge University Press, 2017. Chap. 8. [130]M. D. Shuster. âSurvey of attitude representationsâ. In: Journal of the As- tronautical Sciences 41.4 (1993), p. 439â517. [131]B. Siciliano and O. Khatib. Springer Handbook of Robotics. Springer-Verlag, 2007. [132]B. Siciliano et al. Robotics: Modelling, Planning and Control. Springer Pub- lishing Company, Incorporated, 2008. Chap. 2. [133]B. Siciliano et al. Robotics: Modelling, Planning and Control. Springer Pub- lishing Company, Incorporated, 2008. Chap. 7. [134]B. Siciliano et al. Robotics: Modelling, Planning and Control. Springer Pub- lishing Company, Incorporated, 2008. 2 Open-Loop Control & Trajectory Optimization In Chapter 1, we introduced the state space model as a mathematical formulation for representing a robotâs dynamics. These models, typically expressed as sys- tems of differential equations, provide a foundational framework that describes how the state of a robot evolves over time in response to control inputs. In partic- ular, we saw how a state space model can be derived from the robotâs kinematics and dynamics, providing a compact yet expressive description of its physical mo- tion. In this chapter, we turn to a fundamental question: given a state space model of the robot, how can we determine the control inputs that will drive it to execute a desired behavior? As we will see throughout Chapters 2-4, robots must transform high-level goals into precise physical actions. This process is often described hierarchically, span- ning decision-making, motion planning, trajectory optimization, and control (Chap- ter 2). Each layer plays a distinct role, yet they remain deeply interconnected. At the higher level of this hierarchy, decision-making governs what tasks the robot should perform to fulfill its objectives. It involves reasoning over goals, re- sources, and constraints, often under uncertainty. In a self-driving car, for exam- ple, this may correspond to deciding when to overtake, yield, or reroute. Because it involves strategic considerations rather than immediate actuation, decision- making typically unfolds on the order of seconds to minutes. Ultimately, decision- making defines the high-level objectives that guide subsequent layers of the hier- archy. Once a high-level decision has been made, motion planning determines how to realize it in the robotâs physical environment. This is typically expressed in terms of the robotâs configuration space and involves finding a collision-free path that respects geometric and kinematic constraints. For instance, motion planning may compute a path for a mobile robot to navigate a cluttered warehouse without collisions. The timescale of motion planning is often on the order of hundreds of milliseconds to seconds. Building on this path, trajectory optimization refines it into a time-parameterized trajectory that is dynamically feasible and optimized for performance criteria. This entails solving continuous optimization problems that incorporate dynam- ics, actuator limits, and objectives such as energy efficiency, comfort, or safety This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 56 principles of robot autonomy Figure 2.1: A hierarchical view oftherelationshipbetween decision-making, motion plan- ning,trajectory optimization, and control. margins. The result is a trajectory specifying both the robotâs states and the con- trol inputs needed to realize them over time, ensuring compatibility with the robotâs actuation capabilities and dynamic constraints. Trajectory optimization typically operates on shorter timescales, from tens to hundreds of milliseconds. Finally, low-level control ensures that the robot faithfully executes the desired trajectory in the physical world, by converting the trajectory into actuation com- mands. Controllers must operate at high frequency, applying feedback to correct deviations caused by disturbances, modeling errors, or sensor noise. Whether adjusting wheel torques, joint forces, or thrust vectors, control is what closes the loop between higher-level plans and physical reality. Together, these layers form the backbone of an autonomous system: decision- making provides strategic guidance, motion planning translates that guidance into feasible paths, trajectory optimization refines those paths into feasible and optimal trajectories, and control ensures that the robot can follow those trajec- tories in the real world. In practice, the boundaries between these layers are often blurred. Trajectory optimization, for instance, may be tightly integrated with planning or even embedded within control loops, while high-level decisions may be informed by the lower-level processes. For the purposes of this book, we will adopt the hierarchical perspective outlined above, while acknowledging that real-world systems frequently combine or intertwine these processes. In this chapter, we focus on trajectory optimization as a fundamental tool for computing trajectories that are both feasible and optimal. While control and motion planning will be revisited in Chapter 3 and Chapter 4, respectively, our emphasis in this chapter is on the formulation and solution of the trajectory op- timization problem. We begin in Section 2.1 by introducing the trajectory opti- mization problem and casting it as a continuous optimization problem. Building on this foundation, Sections 2.2 and 2.3 present two major classes of solution strategiesâindirect methods, which derive optimality conditions for the contin- open-loop control & trajectory optimization 57 uous problem using tools from the calculus of variations, and direct methods, which discretize and numerically solve the problem as a finite-dimensional non- linear program. Finally, in Section 2.4, we explore specialized techniques tailored to certain problem structures, known as differentially flat systems. 2.1 The Optimal Control Problem Optimal control theory aims to determine control inputs that drive a dynamical system to satisfy its physical constraints while optimizing a performance crite- rion. At a high level, formulating an optimal control problem requires three key components: âą A mathematical model of the system, typically expressed in state space form. âą A description of the physical constraints the system must satisfy. âą A specification of the performance criterion to be optimized. Mathematical model. As discussed extensively in Chapter 1, the purpose of a mathematical model is to describe how the systemâs state evolves over time in response to control inputs. Using the notation from Chapter 1, the system dy- namics can be expressed as a set of ordinary differential equations: Ìx(t) = f ( x(t), u(t) ) ,(2.1) where x(t) â R n is the state of the system at time t, u(t) â R m is the control input, and f : R n Ă R m â R n defines the state evolution over time. Throughout this book, we will often refer to x(t) and u(t) as the state and control sequences, respectively, or equivalently as the state and control trajectories. Physical constraints. Once we define the system dynamics, the next step is to specify the physical constraints that the system must satisfy. These constraints can take several forms, including: âą Initial conditions, which specify the state at the initial time t 0 as x(t 0 ) = x 0 . âą Final conditions, which specify the state at the final time t f as x(t f ) = x f or x(t f )âX f , whereX f denotes a set of allowable terminal states. âą State constraints, which require that the state remains within an allowable set X for all times tâ [t 0 , t f ], that is, x(t)âX . âą Control constraints, which enforce that the control input remains within an al- lowable setU for all times tâ [t 0 , t f ], that is, u(t)âU . Depending on whether the constraints are satisfied or not, we can define the concept of admissibility for a control history and state trajectory. 58 principles of robot autonomy Definition 2.1.1 (Admissible State and Control Sequences). A state trajectory x(t) and a control sequence u(t) are admissible if they satisfy the state and control constraints at all times, that is: x(t)âXand u(t)âU , âtâ [t 0 , t f ]. Admissibility is a key concept in optimal control, as it restricts the set of real- izable trajectories 1 . 1 In practice, this allows numerical meth- ods to focus exclusively on admissible state and control sequences, rather than considering all possible solutions. Performance criterion. The final component of an optimal control problem is the performance criterion to be optimized. An optimal control is defined as one that minimizes (or maximizes) this performance criterion. In some cases, the perfor- mance criterion may be implicitly defined by the problem statement (e.g., mini- mizing the time to reach a goal state), whereas in other cases, it must be explicitly designed (e.g., driving a car in a way that is comfortable for the passengers). Throughout this book, we will focus on performance criteria that can be ex- pressed as a cost functional 2 of the form: 2 A functional maps functions to real numbers; intuitively, we might say that a functional is a âfunction over functions". Here, the cost functional maps a state trajectory and control sequence to a real number representing the overall cost. J(x(t), u(t), t) = h(x(t f ), t f ) + Z t f t 0 g(x(t), u(t), t) dt,(2.2) where h : R n Ă R â R is the terminal cost and g : R n Ă R m Ă R â R is the running cost. The terminal cost h is evaluated at the final time t f and typically represents a cost associated with the state of the system at that time, such as a penalty for being far from a desired goal state. The running cost g is integrated over the time interval [t 0 , t f ] and represents the instantaneous cost incurred by the system at each time step, for example, the cost of energy consumption, or any other cost associated with the systemâs operation. Depending on the problem, the final time t f may be finite and fixed, finite and free, or infinite 3 . 3 In the case of an infinite final time, the terminal cost h is typically ignored and set to zero. 2.1.1 Problem Formulation As a result, an optimal control problem can be formulated as follows: Determine an admissible control sequence u â (t) such that the system dy- namics: Ìx(t) = f (x(t), u(t), t), generate a corresponding admissible state trajectory x â (t) that minimizes the performance criterion: J(x(t), u(t), t) = h(x(t f ), t f ) + Z t f t 0 g(x(t), u(t), t) dt, where u â (t) and x â (t) are referred to as the optimal control sequence and optimal state trajectory, respectively. open-loop control & trajectory optimization 59 This problem can be formally posed as the following optimization problem: minimize u(t) J(x(t), u(t), t), subject to Ìx(t) = f (x(t), u(t), t), x(t 0 ) = x 0 , x(t f ) = x f , u(t)âU , x(t)âX ,t 0 < t < t f . (2.3) This general formulation serves as the starting point for the optimal control meth- ods developed in the remainder of this book. There are also several important attributes related to the solution of this problem that are worth highlighting: 1. Existence: A solution to an optimal control problem is not guaranteed to exist; there may be no control history that is both admissible and optimal. 2. Uniqueness: Even when a solution exists, it may not be unique. Multiple ad- missible control inputs can yield the same performance. While this can pose challenges for numerical algorithms, it also provides flexibility in selecting among equally good solutions depending on the application. 3. Optimality: The objective of optimal control is to find a control sequence that outperforms all other admissible candidates. Thus, optimal control is inter- ested in global optimality, as opposed to local optimality. Example 2.1.1 (Autonomous Racing Optimal Control). Consider an autonomous racing scenario in which the goal is to complete a lap of a known course in the shortest possible time. We can formulate this as a finite-horizon optimal control problem, where the objective is to minimize the final time t f required to reach a designated goal position (x goal , y goal ), subject to the constraint that the vehicle must remain on the track at all times. Let X denote the set of admissible states that correspond to positions on the course. Suppose the vehicle is modeled using the simple kinematic car model from Equa- tion (1.29), with state x = [ x, y, Ξ ] †representing position and heading, and con- trol inputs u = [ v, Ï ] †representing forward speed and steering angle, respec- tively. The resulting optimal control problem is: minimize v(t), Ï(t) t f , subject to Ì x = v cos Ξ, Ì y = v sin Ξ, Ì Îž = v L tan Ï, xâX course , uâU , x(t 0 ) = x 0 , (x(t f ), y(t f )) = (x goal , y goal ). In this formulation, the cost depends only on the final time and not directly on the state or control at intermediate points. As a result, the time-optimal solution (x(t), u(t)) will lie on the boundaries of the admissible control and state sets. In practice, this means the vehicle will operate at full throttle and steer at the physical limits to cut the lap time, a strategy that achieves optimality mathemat- icallyâbut may not make for a smooth or comfortable ride. 60 principles of robot autonomy Throughout this book, we will see that the solution to the optimal control problem can take different forms, depending on whether and how it incorporates feedback from the current state of the system. At the most fundamental level, we distinguish between open-loop and closed-loop control. While closed-loop control will be the focus of Chapter 3, this chapter addresses open-loop control. Open-loop control. If the optimal control is computed purely as a function of time for a given initial state, u â (t) = â(x(t 0 ), t),(2.4) it is said to be in open-loop form. In the context of trajectory optimization, restrict- ing the focus to open-loop strategies is natural, as they balance computational efficiencyâcomputing open-loop sequences is faster than computing closed-loop policiesâwith effectiveness, since robustness can be endowed through closed- loop tracking or by re-optimizing the trajectory in a receding-horizon fashion, as in Model Predictive Control 4 . 4 Discussed further in Chapter 3. Having introduced the optimal control problem, we now turn to methods for computing optimal open-loop solutions. Fundamentally, Problem (2.3) is an infinite-dimensional optimization problem, where the optimization variables are functions of time. In practice, solution methods must rely on discretization strategies, thereby approximating the infinite-dimensional problem with a finite- dimensional one. Different discretization approaches give rise to distinct families of methods. Broadly speaking, two main classes of methods exist: indirect methods and direct methods. Indirect methods follow an âoptimize-then-discretizeâ paradigm: they first derive necessary conditions for optimalityâtypically in the form of boundary-value problems involving adjoint variablesâand then apply numerical techniques to solve these conditions. In contrast, direct methods take a âdiscretize- then-optimizeâ approach: the state and control sequences are parameterized us- ing finite-dimensional representations, and the resulting finite-dimensional opti- mization problem is solved numerically. Beyond these two general classes, some systems admit further structural sim- plifications. In particular, differentially flat systems allow trajectories to be de- scribed in terms of a small set of variables that fully capture the systemâs evo- lution 5 . This property enables efficient trajectory generation and optimization, 5 These variables are commonly known as flat outputs. making these systems especially relevant in mobile robotics and aerospace appli- cations. The remainder of this chapter examines these three classes of methods in de- tail: indirect methods in Section 2.2, direct methods in Section 2.3, and trajectory optimization for differentially flat systems in Section 2.4. open-loop control & trajectory optimization 61 2.2 Indirect Methods Indirect methods provide a principled framework for solving optimal control problems by drawing on ideas from calculus of variations (CoV) 6 . For a com- 6 Calculus of variations extends the prin- ciples of classical calculus from func- tions to functionals.The central idea is to study how small perturbationsâ called variationsâof a candidate function influence the value of the functional. By analyzing the first- and higher-order ef- fects of these variations, one can derive necessary conditions for optimality. prehensive treatment of the calculus of variations and its applications in optimal control theory, we refer the reader to Kirk [70]. At their core, indirect methods rely on the derivation of necessary optimality conditions (NOCs) that any solu- tion must satisfy and then leverage numerical techniques to compute solutions consistent with these conditions. In this way, the NOCs serve as the bridge be- tween the continuous-time formulation of an optimal control problem and its numerical resolution. Before turning to the derivation of such conditions for infinite-dimensional op- timization problems, let us first review key concepts from finite-dimensional op- timization, which will serve as a foundation for the discussion ahead. 2.2.1 NOCs for Unconstrained Nonlinear Optimization Problems Consider the following finite-dimensional optimization problem: minimize xâR n f (x),(2.5) where f : R n â R is assumed continuously differentiable, i.e., f â C 1 . We wish to identify the NOCs that any minimizerâlocal or globalâmust satisfy. The key intuition is that at a local minimizer, no infinitesimal perturbation of the decision variable should decrease the objective. Formally, this requires analyzing how f changes under small variations around a candidate minimizer x â . First-order necessary condition. Let x â â R n be a local minimizer. If f â C 1 , we can use gradients and Taylor series expansions to characterize the behavior of f near x â . For a small perturbationâx, the cost variation is, up to first order: f (x â +âx)â f (x â )ââ f (x â ) †âx. If x â is a local minimizer, then for sufficiently smallâx, the first-order term must be non-negative 7 : 7 This is because, if we were to decrease the cost by perturbing x â byâx, then x â would not be a local minimizer. â f (x â ) †âx = n â i=1 â f (x â ) âx i âx i â„ 0. In particular, by takingâx to be positive and negative multiples of the coordinate unit vectors, that is, vectors having all components equal to zero except for one component equal to one, we obtain simultaneously: â f (x â ) âx i â„ 0 and â f (x â ) âx i †0,i = 1, . . . , n, which forces the condition: â f (x â ) âx i = 0,i = 1, . . . , n, 62 principles of robot autonomy or, more compactly: â f (x â ) = 0. Thus, any local minimizer x â must be a stationary point of f . Second-order necessary condition. Assuming f â C 2 , consider again the Taylor expansion of f around a local minimizer x â , this time up to second order: f (x â +âx)â f (x â )ââ f (x â ) †âx + 1 2 âx †â 2 f (x â )âx. For x â to be a local minimizer, the second-order variation must be nonnegative for all sufficiently smallâx, that is: â f (x â ) †âx + 1 2 âx †â 2 f (x â )âx â„ 0. Using the first-order conditionâ f (x â ) = 0, the linear term vanishes, leaving: âx †â 2 f (x â )âx â„ 0. Thus, the Hessianâ 2 f (x â ) must be positive semidefinite at any local minimizer. Theorem 2.2.1 (Necessary Conditions for Unconstrained Local Minimizers). Let x â be a local minimizer of f : R n â R. If f â C 1 in an open set containing x â , then: â f (x â ) = 0 (first-order NOC).(2.6) If, in addition, f â C 2 , then: â 2 f (x â )âȘ° 0 (second-order NOC).(2.7) 2.2.2 NOCs for Constrained Nonlinear Optimization Problems Having introduced the NOCs for unconstrained problems, this section extends the discussion to optimization problems subject to constraints. The definition of optimality conditions in the constrained setting requires the introduction of auxiliary variables, known as Lagrange multipliers. These variables are associated with the constraints and facilitate the characterization of optimal solutions while providing insights into the sensitivity of the optimal cost with respect to pertur- bations in the constraints. In this section, we limit our discussion on the theory of Lagrange multipliers to the case of equality constrained optimization. For a com- prehensive treatment of optimality conditions in finite-dimensional optimization, the reader is referred to Bertsekas [13]. Consider the following constrained optimization problem: minimize xâR n f (x), subject toh i (x) = 0,i = 1, . . . , m, (2.8) where f : R n â R and h i : R n â R are continuously differentiable. open-loop control & trajectory optimization 63 For compactness, define the constraint function h : R n â R m as: h(x) = (h 1 (x), . . . , h m (x)) †,(2.9) so that the constraints can be written simply as h(x) = 0. The Lagrange multiplier theorem for equality-constrained optimization states that, if x â is a local minimizer, then there exist scalars λ â 1 , . . . , λ â m , called Lagrange multipliers, such that: â f (x â ) + m â i=1 λ â i âh i (x â ) = 0.(2.10) To interpret this condition, observe that the cost gradientâ f (x â ) must be orthog- onal to the subspace of first-order feasible variations: V(x â ) : =âx|âh i (x â ) †âx = 0,i = 1, . . . , m. This subspace consists of all variationsâx that preserve feasibility to first order 8 . 8 That is, variations for which x = x â + âx satisfies h(x) = 0 to first order. Thus, condition (2.10) ensures that the first-order cost variation â f (x â ) †âx = 0 for allâx â V(x â ). This statement is analogous to the â f (x â ) = 0 condition of unconstrained optimization. Formally, the necessary conditions for equality constrained optimality are sum- marized as follows: Theorem 2.2.2 (Lagrange Multiplier Theorem â Necessary Conditions for Equal- ity Constrained Local Minimizers). Let x â be a local minimizer of f : R n â R subject to the equality constraints h i (x) = 0, i = 1, . . . , m, and assume the constraint gradi- entsâh 1 (x â ), . . . ,âh m (x â ) are linearly independent. Then there exists a unique vector [λ â 1 , . . . , λ â m ] †, called the Lagrange multiplier vector, such that: â f (x â ) + m â i=1 λ â i âh i (x â ) = 0.(2.11) It is often convenient to express these conditions using the Lagrangian function L : R n+m â R defined as: L(x, λ) : = f (x) + m â i=1 λ i h i (x).(2.12) The first-order NOCs for a local minimum x â then take the compact form: â x L(x â , λ â ) = 0, â λ L(x â , λ â ) = 0, (2.13) where â x L and â λ L denote the gradients with respect to x and λ, respectively, and where the system in (2.13) consists of n + m equations in n + m unknownsâ namely, the n components of x â and the m components of λ â . In practice, optimality conditions serve as a powerful tool to filter candidate solutions for global or local minima and often form the foundation of numerical optimization algorithms. For instance, in the unconstrained case of Problem (2.5), 64 principles of robot autonomy one might (i) find all stationary points by solving â f (x) = 0, and (i) apply the second-order test by checking â 2 f (x) âȘ° 0 at each candidate. This same philosophy extends naturally to infinite-dimensional optimal control problems, where any candidate solution must satisfy the corresponding NOCs. However, as we move to infinite-dimensional problems, the nature of the NOCs changes significantly: rather than yielding algebraic equations as in the finite-dimensional case, the NOCs for optimal control take the form of differential equations. 2.2.3 Pontryaginâs Minimum Principle Extending the concept of necessary optimality conditions to infinite-dimensional problems leads to Pontryaginâs Minimum Principle (PMP), a cornerstone of opti- mal control theory. Specifically, the PMP generalizes the finite-dimensional NOCs to the infinite-dimensional setting. Consider the problem of finding an admissible control u â (t) â U that drives the system: Ìx(t) = f (x(t), u(t), t),(2.14) along a trajectory that minimizes the cost functional: J(x(t), u(t), t) = h(x(t f ), t f ) + Z t f t 0 g(x(t), u(t), t) dt. To derive the NOCs, we define the Hamiltonian, the analog of the Lagrangian in finite-dimensional optimization: H(x(t), u(t), p(t), t) : = g(x(t), u(t), t) + p(t) †f (x(t), u(t), t),(2.15) where p(t)â R n is the costate 9 vector. 9 The term costate highlights that there is one costate associated with each state variable, analogous to Lagrange mul- tipliers in finite-dimensional optimiza- tion. Similarly to the finite-dimensional case, where necessary conditions for opti- mality are derived by considering the cost incrementâ f = f (x +âx)â f (x) in response to a perturbationâx, here we analyze the incrementâ J under variations around a candidate function. Theorem 2.2.3 (Pontryaginâs Minimum Principle; for a comprehensive treatment, we refer the reader to Chapter 5 in Kirk [70]). Let u â (t) be an optimal control with associated state trajectory x(t) for the system in (2.14) over [t 0 , t f ]. Then there exists a costate vector p â (t) such that, for all tâ [t 0 , t f ], the following conditions hold: Ìx â (t) = âH âp (x â (t), u â (t), p â (t), t), Ìp â (t) =â âH âx (x â (t), u â (t), p â (t), t), u â (t) = arg min uâU H(x â (t), u, p â (t), t), (2.16) along with the boundary conditions: âh âx x â (t f ), t f â p â (t f ) †Ύx f + H(x â (t f ), u â (t f ), p â (t f ), t f ) + âh ât x â (t f ), t f ÎŽt f = 0, (2.17) open-loop control & trajectory optimization 65 where ÎŽx f and ÎŽt f denote the variations of the final state and time, respectively 10 . 10 As we will discuss in the remainder of this section, the boundary conditions in Equation (2.17) depend on whether the final state and time are fixed (i.e., ÎŽx f = 0 or ÎŽt f = 0) or free (i.e., ÎŽx f or ÎŽt f are arbitrary). Equations (2.16) constitute the necessary conditions for optimality. They form a system of 2n first-order differential equationsân for the state and n for the costateâtogether with m algebraic equations defining the control input. Solving these equations produces 2n constants of integration. Half of these constants are determined by the initial conditions x â (t 0 ) = x 0 . The remaining n (or n + 1, if the final time is free) are specified by the boundary conditions in Equation (2.17). This results in a two-point boundary value problem, which may be solved analytically in special cases, or numerically using methods such as shooting or collocation 11 . 11 J. Hertling.âNumerical Methods for Two-Point Boundary Value Problems (Herbert B. Keller)â. In: SIAM Review 12.2 (1970), p. 313â315 In practice, once the initial state is fixed, the boundary conditions are obtained by substituting the appropriate assumptions into Equation (2.17). Common cases include: Fixed final time and fixed final state. If both t f and x(t f ) are fixed, then ÎŽt f = 0 and ÎŽx f = 0, leaving the sole boundary condition: x â (t f ) = x f . Fixed final time and free final state. If t f is fixed but x(t f ) is free, then ÎŽt f = 0 while ÎŽx f is arbitrary. Hence, the boundary condition is: âh âx x â (t f ), t f â p â (t f ) = 0. Free final time and fixed final state. If x(t f ) is fixed but t f is free, then ÎŽx f = 0 while ÎŽt f is arbitrary. Thus, the boundary condition is: H(x â (t f ), u â (t f ), p â (t f ), t f ) + âh ât (x â (t f ), t f ) = 0. Free final time and free final state. If both x(t f ) and t f are free, then ÎŽx f and ÎŽt f are arbitrary, and both coefficients in Equation (2.17) must be set to zero. That is: âh âx (x â (t f ), t f )â p â (t f ) = 0,(n equations) H(x â (t f ), u â (t f ), p â (t f ), t f ) + âh ât (x â (t f ), t f ) = 0,(1 equation). While these four cases cover many problems of practical interest, more general boundary conditions can be found in Kirk [70]. 2.2.4 Solving a Two-Point Boundary Value Problem Finding solutions that satisfy the necessary optimality conditions in Equation (2.16) is a nontrivial task, as these must simultaneously satisfy a system of 2n differen- tial equations together with boundary conditions imposed at both t 0 and t f . This type of problem, where conditions are specified at two distinct points in time, is known as a two-point boundary value problem (TPBVP). 66 principles of robot autonomy Over the years, a number of numerical procedures have been developed for solving TPBVPs. Two broad classes of approaches are commonly used: âą Shooting methods, which reformulate the TPBVP as an initial value problem by guessing the unknown boundary conditions (e.g., the initial costate), sim- ulating the system forward, and then iteratively adjusting the guess until the terminal boundary conditions are satisfied. Although conceptually straightfor- ward, shooting methods may suffer from numerical instability, especially for long time horizons. âą Collocation methods, whereby the solution is approximated by a parametric function with unknown parameters at a set of discrete points (called colloca- tion points). These methods turn the TPBVP into a large system of nonlinear algebraic equations that can be solved using computational techniques. Collo- cation methods are robust and widely used in practice because they avoid the instability issues of shooting methods. Modern scientific computing environments provide high-level implementations of these ideas. For example, the scikits.bvp _ solver package in Python or the function bvp4c in MATLAB implement numerical algorithms for solving TPBVPs with relatively little effort from the user. Most solvers assume that the system of necessary conditions in Equation (2.16), along with its boundary conditions, can be expressed in the standard form: Ìz = g(z, t),l(z(t 0 ), z(t f )) = 0,(2.18) where z(t) collects the unknown functions (such as states and costates), g encodes their dynamics, and l encodes the two-point boundary constraints. To illustrate how TPBVPs can be solved in practice, consider the toy dynamics: Ìz(t) = " Ì z 1 (t) Ì z 2 (t) # = " z 2 (t) â z 1 (t) # , with boundary conditions z 1 (t 0 ) = 0 and z 1 (t f ) = â2. The boundary conditions can equivalently be expressed in standard form as: l(z(t 0 ), z(t f )) = " z 1 (t 0 ) z 1 (t f ) + 2 # = 0. In Python, the system dynamics g(z, t) and boundary conditions l(z(t 0 ), z(t f )) can be passed directly to solve _ bvp as shown in Algorithm 5. Many optimal control problems can, in fact, be cast into the standard TPBVP form in Equation (2.18) and solved directly with off-the-shelf BVP solvers such as solve _ bvp, sometimes after simple reformulations. Common cases include problems with conditions at special points, such as free-end problems, switching points, interface points, or discontinuities. Example 2.2.1 illustrates these ideas in the context of a free-final-time problem. open-loop control & trajectory optimization 67 Solving a TPBVP with solve _ bvp from scipy.integrate import solve _ bvp import numpy as np # Dynamics: Ì z = g(z, t) def g(t, z): return np.vstack((z[1], -z[0])) # Boundary conditions: l(z(t 0 ), z(t f )) = 0 def l(z0, zf): return np.array([z0[0], zf[0] + 2]) # Time mesh and initial guess for z(t) t _ mesh = np.linspace(0, 4, 5) z _ guess = np.zeros((2, t _ mesh.size)) # Solve TPBVP sol = solve _ bvp(g, l, t _ mesh, z _ guess) z _ sol = sol.sol(np.linspace(0, 4, 100)) Algorithm 5: Example usage of solve _ bvp for a TPBVP in standard form. The code for this example is available in the repository github.com/StanfordASL/por a-exercises in the notebook ch02/tpbvp.ipynb. Example 2.2.1 (Free Final Time Optimal Control Problem; see Example 6.1 in How [54]). Consider the double integrator system: Ì x = u, where x â R is the state and u â R is the control input. The control objective is to find a trajectory that minimizes the cost: J(x, u) = 1 2 αt 2 f + Z t f 0 1 2 ÎČu 2 (t)dt, and satisfies the boundary conditions: x(0) = 10, Ì x(0) = 0,x(t f ) = 0, Ì x(t f ) = 0. This is a free final time problem with fixed boundary conditions on the state. The cost penalizes both the duration of the maneuver (through the αt 2 f term) and the control effort (through the integral of u 2 ), with the trade-off governed by the weights α and ÎČ. We can equivalently express the double integrator dynamics as a first-order system of differential equations by setting x 1 = x and x 2 = Ì x: Ì x 1 = x 2 , Ì x 2 = u, 68 principles of robot autonomy so that the state vector is x = [ x 1 , x 2 ] †and the boundary conditions become: x 1 (0) = 10,x 2 (0) = 0,x 1 (t f ) = 0,x 2 (t f ) = 0. The Hamiltonian is given by: H = 1 2 ÎČu 2 + p 1 x 2 + p 2 u, where p 1 and p 2 are the costate variables. Next, we construct the NOCs from Equation (2.16) by taking the partial derivatives of H with respect to p, x, and u: Ì x â 1 = x â 2 , Ì x â 2 = u â , Ì p â 1 = 0, Ì p â 2 =â p â 1 , 0 = ÎČu â + p â 2 . Thus, from the last condition, the optimal control satisfies: u â =â 1 ÎČ p â 2 . Since this is a free-final-time problem with fixed terminal state, the boundary conditions for the NOCs are given by: x â 1 (0) = 10, x â 2 (0) = 0, x â 1 (t f ) = 0, x â 2 (t f ) = 0, 1 2 ÎČu â (t f ) 2 + p â 1 (t f )x â 2 (t f ) + p â 2 (t f )u â (t f ) + αt f = 0. However, the necessary conditions obtained above do not immediately match the âstandardâ form required by numerical TPBVP solvers in Equation (2.18), which assumes fixed final time. To cast the problem into standard form, one can apply the time-scaling strategy. First, the time horizon is rescaled to the fixed interval [0, 1] by using the scaled time variable Ï = t/t f . Next, the derivatives must be adjusted according to the new time variable. By the chain rule, differentiation with respect to Ï introduces a scaling factor, that is, â âÏ : = â ât â(Ït f ) âÏ = â ât t f . Finally, the final time t f is replaced by an auxiliary state variable r with trivial dynamics Ì r = 0. This results in a TPBVP with fixed final time, namely equal to 1, and an additional state variable r that encodes the original final time t f . For this example, the time-scaled cost becomes: J(x, u, r) = 1 2 αr 2 + Z 1 0 1 2 ÎČru 2 (Ï)dÏ, open-loop control & trajectory optimization 69 with corresponding Hamiltonian: H = 1 2 ÎČru 2 + p 1 rx 2 + p 2 ru. As a result, the necessary conditions for optimality become: Ì x â 1 = r â x â 2 , Ì x â 2 = r â u â , Ì p â 1 = 0, Ì p â 2 =âr â p â 1 , Ì r â = 0, 0 = ÎČr â u â + r â p â 2 , with boundary conditions: x â 1 (0) = 10, x â 2 (0) = 0, x â 1 (1) = 0, x â 2 (1) = 0, αr â + 1 2 ÎČr â u â (1) 2 + r â p â 1 (1)x â 2 (1) + r â p â 2 (1)u â (1) = 0. After this reformulation, the problem adheres to the standard form and can be solved numerically. For a systematic treatment of how nonstandard boundary value problems can be reformulated into standard form suitable for general-purpose solvers, see Ascher and Russell [7]. For a practical implementation of TPBVP solvers for free-final time optimal control problems, we refer the reader to the notebook ch02/free _ final _ time _ optimal _ control.ipynb in the repository github.com/StanfordASL/pora-exercises. 2.3 Direct Methods So far, we introduced indirect methods, which involve deriving the necessary optimality conditions of the continuous-time optimal control problem and then discretizing them to numerically solve the resulting two-point boundary value problem. While indirect methods provide deep theoretical insights, they can be challenging to apply in practice due to the difficulty of solving boundary value problems, particularly for complex nonlinear dynamics or large-scale systems. Direct methods take the opposite approach. Rather than deriving the opti- mality conditions analytically, the problem is discretized first. This reduces the continuous-time optimal control problem to a finite-dimensional nonlinear op- timization problem, which can then be solved using general-purpose numerical optimization algorithms. 70 principles of robot autonomy The process of converting the continuous-time optimal control problem into a discretized form amenable to numerical optimization is known as transcription. While there exist many different transcription methods, a simple and widely used approach is the forward Euler discretization. This method selects a discretization 0 = t 0 < t 1 < . . . < t N = t f of the time interval [0, t f ] and approximates the state and control sequences assuming a zero-order hold on both the states and control inputs, meaning that both the state and the control input are constant over each time interval [t i , t i+1 ). The system dynamics are then integrated forward using Euler integration: x i+1 â x i + h i f (x i , u i ),h i = t i+1 â t i .(2.19) Direct methods are typically grouped into two main families: âą State and control parameterization methods (also known as direct collocation meth- ods): here, both the control inputs and the state trajectories are discretized, and the dynamics are introduced explicitly as algebraic constraints linking the state and control variables at each discretization point (e.g., trapezoidal and Hermite-Simpson collocation, Gauss-Lobatto methods, etc.). âą Control parameterization methods (also known as direct shooting methods): in this approach, only the control inputs are discretized, and the state trajectories are obtained by numerically integrating the system dynamics forward in time. As a result, the optimization variables are solely the discretized controls, while the states are implicitly defined by the integration of the dynamics (e.g., using single or multiple shooting techniques). In what follows, we illustrate the fundamental concepts of both families of methods through a concrete example. Example 2.3.1 (Zermeloâs Problem - Continuous-time problem). Consider the problem of steering a boat from a point (0, 0) to a point (M, â) in a river with a current. The boat can be controlled by adjusting its direction and its speed is constant. The dynamics of the boat are described by the following differential equations: Ì x(t) = v cos(u(t)) + flow(y(t)),tâ [0, t f ], Ì y(t) = v sin(u(t)),tâ [0, t f ], where (x(t), y(t)) is the position of the boat with x defining the coordinate along the river and y the coordinate across the river, u(t) is the control input (the direc- tion of the boat), and v is the constant speed of the boat. For simplicity, assume that the river flow is described by an arbitrary function acting in the x-direction, with its intensity depending on the position y(t), namely flow(y(t)). The objective is to minimize the control effort over time, which can be formu- open-loop control & trajectory optimization 71 lated as an optimal control problem: minimize u(t) Z t f 0 u(t) 2 dt, subject to Ì x(t) = v cos(u(t)) + flow(y(t)),tâ [0, t f ], Ì y(t) = v sin(u(t)),tâ [0, t f ], ( x(0), y(0) ) = (0, 0), x(t f ), y(t f ) = (M, â), |u(t)|†u max ,tâ [0, t f ]. 2.3.1 Direct Collocation Methods Consider the Problem introduced in Example 2.3.1. Applying a state and con- trol parametrization, that is a collocation method, leads to the following finite- dimensional nonlinear program, equivalently described in Algorithm 6: minimize (x,y,u) Nâ1 â i=0 h i u 2 i , subject tox i+1 = x i + h i ( v cos(u i ) + flow(y i ) ) ,i = 0, . . . , Nâ 1, y i+1 = y i + h i v sin(u i ),i = 0, . . . , Nâ 1, (x 0 , y 0 ) = (0, 0), (x N , y N ) = (M, â), |u i |†u max ,i = 0, . . . , Nâ 1. In this formulation, both the state and control trajectories are discretized and treated as decision variables. The system dynamics are not enforced through numerical integration, but rather as algebraic equality constraints linking consec- utive discretization points. 2.3.2 Direct Shooting Methods Many of the concepts introduced for state and control parametrization carry over to control parametrization methods, with a key distinction in how the dynam- ics are handled. In control parametrization (shooting) methods, the optimization variables consist only of the control inputs at each discretization point. The state trajectory is not explicitly optimized but is instead computed recursively by for- ward simulation of the system dynamics. In other words, a candidate sequence of controls uniquely determines the corresponding states, which are then used to evaluate the cost and any state constraints. Concretely, let us revisit Zermeloâs problem from Example 2.3.1, this time using a shooting method transcription. In this formulation, the control inputs u i Nâ1 i=0 are treated as the optimization variables, while the states (x i , y i ) Nâ1 i=0 are computed recursively from the dynamics. 72 principles of robot autonomy Collocation Formulation of Zermeloâs Problem # Decision variables: (x i , y i , u i ), i = 0, . . . , N get _ x = lambda z: z[:N + 1] get _ y = lambda z: z[N + 1:-N] get _ u = lambda z: z[-N:] get _ z = lambda x, y, u: np.concatenate([x, y, u]) # Cost: â Nâ1 i=0 h i u 2 i cost = lambda z: np.sum(h * np.square(get _ u(z))) def constraints(z): x, y, u = get _ x(z), get _ y(z), get _ u(z) constraints = [] for i in range(N): # x i+1 = x i + h i ( v cos(u i ) + flow(y i ) ) constraints.append(x[i+1] - x[i] - h * (v * np.cos(u[i]) + flow(y[i]))) # y i+1 = y i + h i v sin(u i ) constraints.append(y[i+1] - y[i] - h * v * np.sin(u[i])) # Boundary conditions: (x 0 , y 0 ) = (0, 0), (x N , y N ) = (M, â) constraints.extend([x[0], y[0], x[N] - M, y[N] - l]) return np.array(constraints) # State bounds x _ lower = np.zeros(N + 1) x _ upper = M * np.ones(N + 1) y _ lower = np.zeros(N + 1) y _ upper = l * np.ones(N + 1) # Control bounds: u_i†u max ; u _ lower = -u _ max * np.ones(N) # control constraint u _ upper = u _ max * np.ones(N) # control constraint bounds = Bounds( get _ z(x _ lower, y _ lower, u _ lower), get _ z(x _ upper, y _ upper, u _ upper)) # Solve the NLP result = minimize(cost, z0, bounds=bounds, constraints='type': 'eq', 'fun': constraints) Algorithm 6: Direct collocation approach to Zermeloâs problem using forward Euler discretization. The code for this example is available in the repository github.com/StanfordASL/pora-exercises in the notebook ch02/zermelos _ prob lem.ipynb. open-loop control & trajectory optimization 73 The resulting finite-dimensional optimization problem is: minimize u Nâ1 â i=0 h i u 2 i , subject to(x N , y N ) = (M, â), |u i |†u max ,i = 0, . . . , Nâ 1, where, recursively, x i+1 = x i + h i ( v cos(u i ) + flow(y i ) ) ,i = 0, . . . , Nâ 1, y i+1 = y i + h i v sin(u i ),i = 0, . . . , Nâ 1. Here, the dynamics are no longer constraints in the optimization problem, but rather equations that implicitly determine the state evolution given a candidate control sequence. Algorithm 7 provides a Python implementation of this shooting method approach to Zermeloâs problem. Both approaches come with their own advantages and limitations. Control pa- rameterization methods generally result in smaller optimization problems, mak- ing the method computationally attractive. The dynamics are enforced exactly through integration (up to the accuracy of the chosen numerical integrator), which is especially useful when the dynamics are complex or only accessible via a black-box simulator. However, state constraints may be difficult to enforce, as the states are not explicit optimization variables but rather implicitly defined through the integration of the dynamics. This can lead to numerical instability or infeasibility when state constraints are critical. Moreover, errors from numerical integration may accumulate, potentially reducing the accuracy of the solution. On the other hand, state and control parametrization methods treat both states and controls as optimization variables. This allows state constraints to be im- posed directly, improving numerical stability and robustness, often leading to better-conditioned optimization problems when constraints play a central role. However, the resulting optimization problem generally grows significantly in size, since all states and controls at every discretization point are treated as deci- sion variables. This higher dimensionality increases computational cost and can make the solver more sensitive to initial guesses. In practice, both methods are widely used, and the choice between them of- ten depends on the problem structure, the availability of simulators or system models, and the importance of accurately handling state constraints. 2.4 Differentially Flat Systems Computing open-loop control sequences by directly solving optimal control prob- lems can often be computationally intensive. In many applications, it is useful to trade off strict optimality 12 for computational tractability by seeking âgoodâ 12 That is, the theoretical best perfor- mance according to a given cost func- tional. trajectories that are simpler to compute, even if slightly sub-optimal. For a special class of systems known as differentially flat systems, generating such feasible trajectories is considerably simpler. A system is differentially flat if 74 principles of robot autonomy Shooting Formulation for Zermeloâs Problem # Decision variables: (u i ), i = 0, . . . , Nâ 1; Cost: â Nâ1 i=0 h i u 2 i cost = lambda u: np.sum(h * np.square(u)) # States computed recursively from x 0 = 0, y 0 = 0 dynamics = lambda x, y, u: ( x + h * (v * np.cos(u) + flow(y)), y + h * v * np.sin(u) ) def inequality _ constraints(u): x, y = 0, 0 # initial condition (x(0), y(0)) = (0, 0) constraints = [] for ui in u: x, y = dynamics(x, y, ui) # (x i , y i ) >= (0, 0) (box constraint with below) constraints.extend([x, y]) # (x i , y i ) <= [M, â] constraints.extend([M - x, l - y]) # (x N , y N ) >= [M, â] (enforcing equality with the above) constraints.extend([x - M, y - l]) return constraints bounds = Bounds(-u _ max * np.ones(N), u _ max * np.ones(N)) # |u i |†u max # Solve NLP result = minimize(cost, u0, bounds=bounds, constraints='type': 'ineq', 'fun': inequality _ constraints) Algorithm 7: Direct shooting approach to Zermeloâs problem using forward Euler discretization. The code for this example is available in the repository github.com /StanfordASL/pora-exercises in the notebook ch02/zermelos _ problem.ipynb. there exists a set of outputs, called flat outputs, such that all system states and in- puts can be expressed as algebraic functions of these outputs and a finite number of their derivatives. This property allows trajectory generation to be performed in the space of the flat outputs, eliminating the need to solve differential equations as part of the optimization process and greatly reducing computational complex- ity. Differentially flat models arise in several common robotics applications, in- open-loop control & trajectory optimization 75 cluding simple car models, quadrotors, and many other wheeled or aerial vehi- cles. Their relative simplicity and expressiveness make them particularly attrac- tive for trajectory planning and open-loop control synthesis. Example 2.4.1 (Differentially Flat Autonomous Vehicle Control). Recall the mo- tion planning task from Example 2.1.1 where the objective was to compute an open-loop control sequence to drive a vehicle through a course to a goal position in minimum time. If we relax the requirement of optimality and instead aim simply to find a feasible trajectory that follows the course, we can exploit the dif- ferential flatness of the kinematic car model. Specifically, consider the kinematic car model from Equation (1.29): Ì x = v cos Ξ, Ì y = v sin Ξ, Ì Îž = v L tan Ï, where (x, y) is the vehicle position, Ξ is the heading, v is the speed, Ï is the steering angle, and L is the wheelbase. This system is differentially flat with flat outputs (x(t), y(t)). Therefore, it is sufficient to specify any differentiable trajectory for x(t) and y(t) that respects the course constraints. From these trajectories, the remaining state and control variablesâwhich are the quantities needed for practical implementationâcan be computed analytically. The heading is obtained from the velocity direction as: Ξ = tan â1 Ì y Ì x . and the speed along the trajectory can be computed using either component of the velocity: v = Ì x cos Ξ ,orv = Ì y sin Ξ . Finally, the steering angle is determined from the heading dynamics: Ï = tan â1 L Ì Îž v . In this way, a feasible trajectory for the vehicle can be generated entirely by spec- ifying smooth flat output trajectories, from which all states and, importantly, the control inputs can be derived directly. We formalize this concept through the notion of differential flatness 13 . 13 Murray [101] is a good resource for a comprehensive treatment on differential flatness. Definition 2.4.1 (Differential Flatness). A nonlinear system with state xâ R n and control uâ R m : Ìx(t) = f (x(t), u(t)),(2.20) is differentially flat if there exists a function α such that: z = α(x, u, Ìu, . . . , u (a) ),(2.21) 76 principles of robot autonomy where u (i) denotes the i-th time derivative of u, and such that the system trajecto- ries can be expressed as functions of the flat output z â R m and a finite number of its derivatives: x = ÎČ(z, Ìz, . . . , z (b) ) u = Îł(z, Ìz, . . . , z (c) ). (2.22) In other words, a system is said to be differentially flat if there exists a set of outputs z (with the same dimension as the input vector u) that completely deter- mine both the states and the inputs, without requiring integration of the system dynamics. For trajectory optimization, this property is particularly advantageous: since the evolution of a flat system is fully characterized by its flat outputs, tra- jectories can be computed directly in the output space and then mapped to the corresponding inputs, thereby avoiding expensive integration of the dynamics. In the following sections, we explore different techniques to exploit differential flatness for open-loop trajectory design, including how to parameterize trajecto- ries in the flat output space, handle initial and terminal state constraints, and enforce control constraints. 2.4.1 Trajectory Parameterization Our primary limitation when planning a trajectory in the flat output space is that it must be differentiable. A common approach is to parameterize each component of the flat output z using N smooth basis functions: z j (t) = N â i=1 α [j] i Ï i (t),(2.23) where z j is the j-th element of z, α [j] i â R are parameters that define the trajectory, and Ï i (t) are smooth basis functions. Polynomial basis functions are a natural choice, e.g., Ï 1 (t) = 1, Ï 2 (t) = t, Ï 3 (t) = t 2 , etc. A key advantage of this parameterization is that z j (t) is linear in the variables α [j] i , which facilitates translating constraints on z and its derivatives directly into constraints on the coefficients α [j] i . 2.4.2 Equality Constraints A key component of any open-loop motion planning problem is the enforcement of boundary conditions. Typically, this means ensuring that the system begins at a prescribed initial state x(0) = x 0 and often that it reaches a desired terminal state, x(t f ) = x f , at some final time t f . When planning in the flat output space, these state conditions must be expressed as constraints on the flat output z(t) and its derivatives. Recalling the mapping in Equation (2.22), this leads to: x 0 = ÎČ(z(0), Ìz(0), . . . , z (q) (0)), x f = ÎČ(z(t f ), Ìz(t f ), . . . , z (q) (t f )). (2.24) open-loop control & trajectory optimization 77 In practice, this means that boundary conditions on z j (0), Ì z j (0), . . . , z (q) j (0) and z j (t f ), Ì z j (t f ), . . . , z (q) j (t f ) must be enforced. When using a smooth basis function parameterization of the form in Equation (2.23), these conditions translate directly into algebraic constraints on the coefficients α [j] i . By differentiating Equation (2.23) q times, we obtain: Ì z j (t) = N â i=1 α [j] i Ì Ï i (t), . . . z (q) j (t) = N â i=1 α [j] i Ï (q) i (t). (2.25) which allows us to express the boundary conditions as a system of linear equa- tions: ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° Ï 1 (0)Ï 2 (0). . .Ï N (0) Ì Ï 1 (0) Ì Ï 2 (0). . . Ì Ï N (0) . . . . . . . . . Ï (q) 1 (0)Ï (q) 2 (0). . .Ï (q) N (0) Ï 1 (t f )Ï 2 (t f ). . .Ï N (t f ) Ì Ï 1 (t f ) Ì Ï 2 (t f ). . . Ì Ï N (t f ) . . . . . . . . . Ï (q) 1 (t f ) Ï (q) 2 (t f ). . . Ï (q) N (t f ) ïŁč ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁ» ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° α [j] 1 α [j] 2 . . . α [j] N ïŁč ïŁș ïŁș ïŁș ïŁș ïŁș ïŁ» = ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° z j (0) Ì z j (0) . . . z (q) j (0) z j (t f ) Ì z j (t f ) . . . z (q) j (t f ) ïŁč ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁ» .(2.26) Assuming the matrix formed by the basis functions has a sufficient number of columns and that it is full column rank, we can solve for (possibly non-unique) α [j] i that solve the trajectory generation problem. More generally, any equality constraint on the flat outputs or their deriva- tivesâbeyond just initial and terminal statesâcan be written in this linear form. For instance, waypoints can be added as additional equality constraints. How- ever, if too many constraints are imposed, the system may become overdeter- mined, leaving no feasible solution. In such cases, one must increase the richness of the basis functions, for example by using higher-order polynomials or ad- ditional functions, which improves flexibility but also increases computational complexity. 2.4.3 Inequality Constraints via Time Scaling Having addressed equality constraints in Section 2.4.2, we now turn to inequality constraints. These commonly arise in motion planning and control to enforce actuator limits or safety bounds on the state. For example, the simple car model from Example 2.4.1 may have a speed constraint of the form: |v(t)|†v max . 78 principles of robot autonomy A useful technique for handling such constraints in the flat-output space is time scaling. The idea is to first plan a trajectory that satisfies the equality con- straints (e.g., by solving Equation (2.26)), and then adjust its temporal evolu- tionâspeeding up or slowing down along the pathâto enforce the inequality constraints. Formally, let x(t) be a trajectory satisfying the equality constraints. We can separate the geometric path 14 from its timing by introducing a path parameter 14 The geometric path of a trajectory is the sequence of states x of the trajectory, but not associated with a particular time s(t): x(t) = x(s(t)), with s(0) = s 0 , s(t f ) = s f , and Ì s(t) > 0 15 . The geometric path x(s) captures the 15 The condition Ì s(t) > 0 ensures invert- ibility, so each t corresponds to a unique s. sequence of states, while the choice of s(t) determines how quickly the system traverses that path. Varying s(t) is referred to as time scaling. Example 2.4.2 (Time Scaling for a Simple System). Consider a scalar system with state x â R and a straight-line path connecting an initial and terminal state, x 0 and x f : x(s) = x 0 + s(x f â x 0 ),sâ [0, 1]. Choosing a cubic polynomial for s(t), s(t) = 3 T 2 t 2 â 2 T 3 t 3 ,tâ [0, T], which satisfies s(0) = 0, s(T) = 1, and Ì s(t) > 0, yields the temporal trajectory: x(t) = x 0 + 3 T 2 t 2 â 2 T 3 t 3 (x f â x 0 ). Here, T controls the duration of the trajectory. If we impose a velocity bound: | Ì x|â€ Ì x max , then: Ì x = 6 t T 2 â t 2 T 3 (x f â x 0 ), Ì x = 6 1 T 2 â 2t T 3 (x f â x 0 ), with the maximum velocity attained at t = T 2 . We can then convert this into a constraint on T to ensure the inequality constraint is satisfied: T â„ 3(x f â x 0 ) 2 Ì x max . Example 2.4.2 illustrates the key idea: inequality constraints can often be trans- formed into conditions on the timing law s(t), without altering the geometric path itself. open-loop control & trajectory optimization 79 For general state-space systems, time scaling is often more complex. Given a feasible trajectory (x(t), u(t)) of the system dynamics in Equation (2.20), we can rewrite it as a geometric path (x(s), u(s)) using a path parameter s(t): dx(s) ds ds(t) dt = f (x(s), u(s)).(2.27) For time scaling, we replace s(t) with a new path parameter Ì s(t) over a possibly different interval t â [0, Ì t f ], with Ì s(0) = s 0 and Ì s( Ì t f ) = s f 16 . The new scaling 16 The geometric path is still defined on the interval [s 0 , s f ], which must remain the same for any new time scaling law. must still satisfy the dynamics: dx( Ì s) d Ì s d Ì s(t) dt = f (x( Ì s), u( Ì s)).(2.28) Since the geometric path is fixedâas it was previously definedâthe terms dx( Ì s) d Ì s and x( Ì s) are also fixed. Therefore, time scaling with a new path parameter Ì s(t), is only admissible if an appropriate Ìu( Ì s) can be found. Fortunately, for many systemsâincluding those commonly studied in motion planningâthis is possible with the right choice of Ì s(t). Example 2.4.3 (Time Scaling for the Simple Car Model). Consider again the sim- ple car model from Equation (1.29): Ì x = v cos Ξ, Ì y = v sin Ξ, Ì Îž = v L tan Ï, and suppose we have identified a candidate trajectory x c (t) with control u c (t) by leveraging the differential flatness of the model through Equation (2.26) and mapping the flat outputs z c (t) into the state and control space. For this model, a natural choice for the path parameter s is the arc-length, defined as: s(t) = Z t 0 v(Ï)dÏ. such that Ì s(t) = v(t) > 0. With this choice, the geometric path x c (s) is defined over s â [0, L path ], where L path is the total length of the path. Rewriting the dynamics in terms of an arbitrary time scaling Ì s(t) gives: dx c ( Ì s) d Ì s Ì Ì s = v( Ì s) cos Ξ c ( Ì s), dy c ( Ì s) d Ì s Ì Ì s = v( Ì s) sin Ξ c ( Ì s), dΞ c ( Ì s) d Ì s Ì Ì s = v( Ì s) L tan Ï( Ì s), which must hold for any admissible time scaling Ì s(t) 17 . 17 The trivial choice Ì s(t) = s(t) repro- duces the original candidate trajectory with control inputs u c (t). 80 principles of robot autonomy By adopting the arc-length parameterization, we have Ì Ì s = v( Ì s), so these equa- tions reduce to: dx c ( Ì s) d Ì s = cos Ξ c ( Ì s), dy c ( Ì s) d Ì s = sin Ξ c ( Ì s), dΞ c ( Ì s) d Ì s = 1 L tan Ï( Ì s). The first two equations are automatically satisfied for any choice of Ì s â [s 0 , s f ], since the original candidate trajectory satisfies the dynamics. On the other hand, the third equation is satisfied provided we reuse the same steering input, Ï( Ì s) = Ï c ( Ì s). Therefore, the dynamics remain consistent for any choice of time scaling Ì s(t): the geometric path is preserved, while the temporal evolution along that path is left free. This observation is powerful as we may freely adjust the speed input v(t), subject only to Ì Ì s(t) > 0, without altering the geometry of the trajectory. In practice, this allows us to easily enforce inequality constraints on the speed |v(t)|†v max . Example 2.4.3 shows a relatively straightforward application of time scaling to a model derived from kinematic constraints. This idea extends naturally to a wide class of kinematic models of the form: Ìx(t) = G(x(t))u(t).(2.29) Applying the chain rule, we obtain: dx(s) ds Ì s = G(x(s(t)))u(t), which can be rewritten as: dx(s) ds = G(x(s))u g (s),(2.30) where u g (s) = u(t) Ì s(t) is the geometric control 18 . Equation (2.30) shows that the geo- 18 Since s(t) must be strictly increasing, we require Ì s(t) > 0. metric path x(s) is fully determined by the geometric control u g (s), independent of the time parametrization. Therefore, once the geometric control and geomet- ric path are defined, we can temporally scale the trajectory x(t) using the path parameter s(t) without changing the geometric path. The corresponding control inputs are recovered via u(t) = Ì s(t)u g (s). In summary, for models of the form (2.29), we can perform time scaling by: 1. Selecting a path parameter s (e.g., arc-length), computing s(t) for the original trajectory x(t), and determining the interval [s 0 , s f ]. 2. Re-parameterizing the control u(t) in terms of s. 3. Computing the geometric control u g (s) = u(s(t))/ Ì s(t) for sâ [s 0 , s f ]. open-loop control & trajectory optimization 81 4. Defining a new path parameter function Ì s(t) over the interval [0, Ì t f ] with Ì Ì s(t) > 0, Ì s(0) = s 0 , and Ì s( Ì t f ) = s f . 5. Computing the new control inputs as Ìu(t) = u g ( Ì s(t)) Ì Ì s(t) for all tâ [0, Ì t f ]. Example 2.4.4 (Time Scaling for the Unicycle Model). Consider the kinematic unicycle model: Ì x = v cos Ξ, Ì y = v sin Ξ, Ì Îž = Ï, (2.31) where ( x, y ) denote the position, Ξ the heading, v the forward velocity, and Ï the rotation rate. We define the state as x = [ x, y, Ξ ] †and the control as u = [ v, Ï ] †. A natural path parameter for this system is again the arc-length: s(t) = Z t 0 v(Ï)dÏ, such that Ì s(t) = v(t) > 0. If the trajectory is defined over t â [0, T] with total length L path , then s(0) = 0 and s(T) = L path . The corresponding geometric controls are: v g (s) = v(s) Ì s(t) = 1, Ï g (s) = Ï(s) Ì s(t) = Ï(s) v(s) , where the fact that v g (s) = 1 follows directly from Ì s(t) = v(s(t)). Introducing a new timing law Ì s(t) generates a new velocity profile Ì v( Ì s) = Ì Ì s(t) along the path, which can use to solve for the new Ì Ï inputs by: Ì Ï( Ì s) = Ï g ( Ì s) Ì Ì s(t) = Ï( Ì s) v( Ì s) Ì v( Ì s). In practice, it is often simpler to directly prescribe a velocity profile Ì v( Ì s) along the path and compute the corresponding angular velocity Ì Ï( Ì s) = Ï( Ì s) v( Ì s) Ì v( Ì s). Finally, to determine the new controls as functions of time, we note that: Ï(s) = Z s 0 1 Ì v(s âČ ) ds âČ , defines a function Ï(s) that maps each point sâ [0, L path ] to a new time. Example 2.4.5 (Planar Quadrotor Control). In this example we consider the con- trol of a planar quadrotor system. The quadrotor is modeled with six state vari- ables: horizontal position x, vertical position y, orientation angle Ï, and their respective velocities. The control inputs are the thrusts T 1 and T 2 from the two rotors. The objective is to minimize the energy consumption, represented by the integral of the squared thrusts over time: minimize Z t f 0 T 1 (t) 2 + T 2 (t) 2 dt. 82 principles of robot autonomy The system dynamics are given by the following differential equations: ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° Ì x Ì v x Ì y Ì v y Ì Ï Ì Ï ïŁč ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁ» = ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° v x â (T 1 +T 2 ) m sin Ï v y (T 1 +T 2 ) m cos Ïâ g Ï (T 2 âT 1 )â I z ïŁč ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁș ïŁ» , where m is the mass, g is the gravitational acceleration, â is the distance from the center of mass to each rotor, and I z is the moment of inertia about the z-axis. This system is differentially flat, with flat outputs (x, y). For a practical imple- mentation of differential flatness for trajectory generation applied to this system, refer to the notebook ch02/differentially _ flat _ planar _ quadrotor.ipynb in the repository github.com/StanfordASL/pora-exercises. 2.5 Summary In this chapter, we explored how trajectory optimization provides a fundamental framework for computing open-loop motions and establishing a foundation for autonomous decision-making in robotic systems. We began by formalizing the optimal control problemâa mathematical for- malization for the task of driving a systemâs state evolution through admissible control inputs while optimizing a performance criterion. This formulation in- volves three key components: the systemâs mathematical model, the physical constraints, and the performance criteria. We then introduced two major families of methods for solving optimal con- trol problems and compute optimal open-loop control sequences: direct meth- ods and indirect methods. Indirect methods adopt an âoptimize-then-discretizeâ approach, deriving analytical necessary conditions for optimality and solving the resulting two-point boundary value problem numerically. In contrast, direct methods follow a âdiscretize-then-optimizeâ strategy, transcribing the continu- ous problem into a finite-dimensional nonlinear program that can be solved with standard optimization solvers. Lastly, we discussed differentially flat systems, a special class of systems for which trajectory generation is significantly simplified. For these systems, plan- ning can be performed in a lower-dimensional âflat output" space using tech- niques like polynomial parameterization. We showed how initial, terminal, and waypoint constraints can be translated into linear algebraic equations, and how inequality constraints on state and control can be managed through time scaling. To learn more. For comprehensive treatments of optimal control, we point the reader to several excellent references, including Murray [101], Kirk [70], Rao [117], and Kelly [67]. Kirk [70] offers a foundational perspective on indirect meth- ods, including detailed derivations of the calculus of variations and Pontryaginâs open-loop control & trajectory optimization 83 Minimum Principle. For a thorough exploration of direct methods and mod- ern optimization-based approaches, Rao [117] and Kelly [67] provide in-depth coverage of transcription techniques, such as collocation and shooting, and their formulation as nonlinear programming problems. Finally, Murray [101] gives an extensive overview of differential flatness, illustrating how this property can be exploited for efficient trajectory generation. 2.6 Exercises The starter code for the exercises provided below is available online through GitHub. To get started, download the code by running in a terminal window: git clone https://github.com/StanfordASL/pora-exercises.git We denote Problems requiring hand-written solutions and coding in Python withand, respectively. Problem 1: Extremal Curves [This exercise is inspired by Kirk [70], Chapter 4, Problem 4.9] Given the functional: J(x) = Z 1 0 1 2 Ì x(t) 2 + 5x(t) Ì x(t) + x(t) 2 + 5x(t) dt, find an extremal curve x â : [0, 1]â R that satisfies x â (0) = 1 and x â (1) = 3. Problem 2: Minimum Control Effort Consider the dynamics: Ì x(t) =â2x(t) + u(t), with the initial constraint x(0) = 2, terminal constraint x(1) = 0, and cost func- tional: J(u) = Z 1 0 u(t) 2 dt. Write down the Hamiltonian and use the necessary optimality conditions to de- rive an optimal control u â (t) and corresponding state trajectory x â (t). Problem 3: Zermeloâs Ship Zermeloâs ship must travel through a region of strong currents. The position of the ship is denoted by (x(t), y(t)) â R 2 . The ship travels at a constant speed v > 0, yet its heading Ξ(t) can be controlled. The current moves in the positive 84 principles of robot autonomy x-direction with speed w(y(t)). The equations of motion for the ship are: Ì x(t) = v cos Ξ(t) + w(y(t)), Ì y(t) = v sin Ξ(t). We want to control the heading Ξ(t) such that the ship travels from a given initial position (x(t 0 ), y(t 0 )) = (x 0 , y 0 ) to the origin (0, 0) in minimum time. 1. Suppose w(y(t)) = v h y(t), where h > 0 is a known constant. Show that an optimal control law Ξ â (t) must satisfy a linear tangent law of the form: tan Ξ â (t) = αâ v h t, for some constant αâ R. 2. Suppose w(y(t)) ⥠ÎČ for some constant ÎČ > 0. Derive an expression for the optimal transfer time t â 1 â t 0 . Problem 4: Singular Arc for Dubinsâ Car The kinematics of Dubinsâ car are described by: Ì x = v cos Ξ, Ì y = v sin Ξ, Ì Îž = u, where (x, y) â R 2 is the carâs position, Ξ â R is the carâs heading, v > 0 is the carâs constant known speed, and u is the controlled turn rate. The turn rate is bounded, meaning uâ [â Ì Ï, Ì Ï], where Ì Ï > 0 is a known constant. The car starts at (x, y) = (0, 0) with a heading of Ξ = 0 at t = 0. We want the car to drive to (x, y) = (0, c) in the least amount of time possible, where c > 0 is a given constant. 1. Use Pontryaginâs maximum principle to express the optimal control input u â (t) as a function of the optimal co-state p â (t) : = ( p â x (t), p â y (t), p â Ξ (t))â R 3 . Hint: You should discover that the maximum condition for u â (t) is not infor- mative whenever p â Ξ (t) âĄ Ì p Ξ for a particular fixed value Ì p Ξ â R. When such a lack of information persists over a non-trivial time interval, i.e., any time in- terval [t 1 , t 2 ] with t 2 > t 1 â„ 0, this is known as a singular arc. To compute u â (t) in this case, use the fact that p â Ξ (t) âĄ Ì p Ξ is constant in time along this singular arc. 2. Use boundary conditions to argue why p â (t) might end in a singular arc. Suppose we know p â (t) begins on a non-singular arc, then switches once to and ends on a singular arc. For this particular case, argue why u â (0) = Ì Ï and describe the optimal state trajectory (x â (t), y â (t), Ξ â (t)) and control trajectory u â (t) in words without explicitly deriving them. open-loop control & trajectory optimization 85 Problem 5: Single Shooting for a Unicycle Consider the kinematic model of a unicycle: Ì x = v cos(Ξ), Ì y = v sin(Ξ), Ì Îž = Ï, where (x, y) is the planar position of the vehicle, Ξ is its heading angle, v is its forward velocity, and Ï is its angular velocity. Overall, the state and control input for this system are x : = (x, y, Ξ)â R 3 and u : = (v, Ï)â R 2 , respectively. We have overloaded x to denote both horizontal position x â R and the full state vector x â R 3 . Our task is to drive the vehicle from the starting configuration x(0) = (0, 0, Ï/2) to the target configuration x(T) = (5, 5, Ï/2) in minimum time with as little con- trol effort as possible. To this end, we consider the objective: J(x, u) = Z T 0 α + v(t) 2 + Ï(t) 2 dt, where α > 0 is a chosen constant weighting factor and T is the free final time. 1. Derive the Hamiltonian and necessary optimality conditions, specifically (a) the ODE for the state and co-state, (b) the optimal control as a function of the state and co-state, and (c) the boundary conditions, including the additional condition for free final time T. Hint: Since the control set is unbounded, use the weak maximum condition. In practice, you might use a boundary value problem (BVP) solver from an existing computing library, such as scipy.integrate.solve _ bvp, but in this problem we will use a bit of nonlinear optimization theory and JAX to write our own! 2. In the file ch02/exercises/unicycle _ optimal _ control.ipynb, complete the implementations of unicycle _ dynamics, hamiltonian, optimal _ control, and noc _ ode. Use α = 0.25. In the single shooting method, we need to initialize estimates of the initial co- state p(0) and final time T. We then integrate the state and co-state dynamics forward in time from t = 0 to t = T, at which point we check whether the terminal boundary conditions are satisfied. 3. Use the ODE integration from noc _ trajectories to complete boundary _ residual, which should compute a measure of how far off each of your terminal boundary conditions is from satisfaction, given guesses for the initial co-state p(0) and final time T. 86 principles of robot autonomy 4. Finally, in newton _ step and single _ shooting, implement the Newton- Raphson root-finding method for boundary _ residual. Now, if you provide an appropriate guess for the initial costate and final time, you can solve the problem in unicycle _ optimal _ control.ipynb and see a plot of the optimal solution. You may find that whether or not your BVP solver converges to a solution is highly dependent on the quality of your initial guessâindeed, ini- tialization is a major challenge when applying indirect methods for optimal control! Hint: For finding roots of a function f : R n â R n , each iteration of the Newton-Raphson method entails improving a current best guess x (k) at it- eration k using the update rule: x (k+1) = x (k) â â f âx (x (k) ) â1 f (x (k) ). open-loop control & trajectory optimization 87 References [7]U. M. Ascher and R. D. Russell. âReformulation of boundary value prob- lems into âstandardâ formâ. In: SIAM Review 23.2 (1981), p. 238â254. [13]D. Bertsekas. Nonlinear Programming. Athena Scientific, 2016. [51]J. Hertling. âNumerical Methods for Two-Point Boundary Value Problems (Herbert B. Keller)â. In: SIAM Review 12.2 (1970), p. 313â315. [54]J. P. How. Lecture Notes for Principles of Optimal Control. 2008. [67]M. Kelly. âAn Introduction to Trajectory Optimization: How to Do Your Own Direct Collocationâ. In: SIAM Review 59.4 (2017), p. 849â904. [70]D. E. Kirk. Optimal Control Theory: An Introduction. Dover Publications, 2004. [101]R. M. Murray. Optimization-Based Control. California Institute of Technol- ogy, 2009. [117]A. Rao. âA Survey of Numerical Methods for Optimal Controlâ. In: Ad- vances in the Astronautical Sciences 135 (2010). 3 Closed-Loop Control & Trajectory Tracking In Chapter 2, we introduced trajectory optimization as a foundational tool for open-loop optimal control, where the objective is to compute a time-parameterized control sequence that minimizes a given cost function subject to system dynamics and constraints. Within a robotâs autonomy stack, open-loop control is often a key component, generating control sequences for tasks such as manipulation, locomotion, and navigation. However, because open-loop strategies depend only on time and not on the current system state, they are inherently vulnerable to disturbances, modeling errors, and other execution-time uncertainties. In this chapter, we turn our attention to closed-loop optimal control. While the core elements of the problem remain the sameânamely, the system dynamics, state and input constraints, and a performance criterionâthe object of optimiza- tion is fundamentally different. Specifically, rather than seeking an open-loop sequence u(t), closed-loop control aims to determine an optimal control law of the form u(t) = Ï(x(t), t), which explicitly depends on the current state in ad- dition to the current time. Such closed-loop control laws are often referred to as feedback controllers or control policies. Obtaining solutions to an optimal control problem in the form of a closed-loop policy is particularly powerful, as it provides a rule prescribing optimal behav- ior from any state the system might encounter. Under idealized assumptionsâ that is, exact system models, perfect knowledge of the state, and absence of disturbancesâthe optimal open-loop sequence and the optimal closed-loop pol- icy would produce identical behavior. In practice, however, such conditions are rarely met, with real systems inevitably facing model uncertainties, external dis- turbances, and measurement noise. In these settings, closed-loop policies offer a distinct advantage, as they continuously adapt the control input to the actual system state, thereby providing levels of robustness that are difficult to achieve through open-loop strategies (Chapter 3). Formally, we define a closed-loop control law as follows: Definition 3.0.1 (Closed-loop control law). A closed-loop control law is a function Ï : R n Ă R â R m that maps the current state 1 x(t) â R n and time t â R to a 1 When the full system state is not di- rectly measurable, closed-loop control laws can be defined based on the current measured system outputs, i.e., u(t) = Ï(y(t), t), where y(t) denotes the mea- sured system output at time t. In this case, the closed-loop policy is referred to as an output feedback policy. This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 90 principles of robot autonomy Figure 3.1: Comparison between open-loop and closed-loop con- trol in a 2D gridworld naviga- tion task. Without disturbances (left figure), we can see both open and closed-loop policies lead to same path, ending at the goal (star).With a wind dis- turbance (right figure), the fixed series of actions from the open- loop control sequence fails to take the robot to the goal after it is pushed upward by the wind. In contrast, the closed-loop pol- icy specifies an action for each state, allowing the system to re- act to the disturbance and suc- cessfully reach the goal by ad- justing its behavior based on the current state. control input u(t)â R m : u(t) = Ï(x(t), t).(3.1) Despite their theoretical appeal and practical advantages, the main drawback of closed-loop control approaches is computational, as solving for an optimal closed-loop policy is generally more expensive than computing an open-loop in- put sequence. In practice, a useful compromise is achieved by following a two- step design, which aims to combine the strengths of both open- and closed-loop paradigms. In this scheme, one first computes a nominal reference trajectory by solving an open-loop optimal control problem, using tools such as those intro- duced in Chapter 2. Then, a closed-loop controller is designed to track this refer- ence trajectory during execution. Conceptually, the resulting controller blends a feedforward termâthe nominal trajectoryâwith a feedback term that reacts to de- viations from it. As we will discuss in more depth throughout this chapter, these ideas are at the core of what is commonly referred to as trajectory tracking control. Formally, the two-step design can be expressed as: u(t) = Ìu(t) + Ï(x(t), Ìx(t), t),(3.2) where Ìu(t) and Ìx(t) denote the nominal input and state trajectories, and Ï is a trajectory-tracking control law ensuring that the actual trajectory remains close to the planned one despite model mismatch or disturbances. Building on these concepts, this chapter explores key strategies for closed-loop optimal control and their application to trajectory tracking. We begin in Sec- tion 3.1 with the classical paradigm of feedback control, illustrated through the widely used proportionalâintegralâderivative (PID) controllers and their application to tracking, with a focus on differentially flat systems. Next, in Section 3.2, we closed-loop control & trajectory tracking 91 turn to linear optimal control, focusing on the linear quadratic regulator (LQR), its key extensions, and its application to tracking control. We then discuss nonlinear optimal control problems in Section 3.3, first introducing techniques that compute optimal solutions in closed-loop formâsuch as the Hamilton-Jacobi-Bellman (HJB) equation and Dynamic Programming (DP)âand then showing how, for tracking purposes, linear methods can be extended to nonlinear systems through lineariza- tion. Building on this, we present how LQR ideas generalize to nonlinear opti- mal control through algorithms such as iterative LQR (iLQR) and differential dy- namic programming (DDP), that simultaneously generate an open-loop trajectory and closed-loop tracking controller in two-step design form. Finally, in Sec- tion 3.4, we introduce model predictive control (MPC), a powerful framework that brings together ideas of open-loop and closed-loop optimal control through the framework of receding horizon optimization, and discuss how it can be applied to tracking. 3.1 Classical Feedback Control The central goal of classical feedback control is to regulate a systemâs output so that it tracks a desired reference signal. For example, in the context of an autonomous vehicle, one might want to control the vehicleâs speed to match a target velocity or maintain a specific distance from another vehicle. The classical paradigm of feedback control is built on a simple yet power- ful principle: continuously measure the system output, compare it against the desired reference, and compute corrective actions based on the resulting discrep- ancy, known as the error signal. These corrective inputs are then applied to the system, ensuring the output continually adjusts toward the desired behavior. For example, accelerate if the vehicle is below the target speed, or decelerate it if it is above. Classical feedback controllers are generally designed to satisfy a set of funda- mental desiderata: âą Stability: the controller should ensure that the system remains stable. While there are several formal notions of stability, the essential requirement is that, loosely speaking, the system âis bounded in its behaviorâ. âą Tracking: the controller should minimize the error between the system out- put and the desired reference, ensuring the system follows the commanded trajectory as closely as possible. âą Disturbance rejection: the controller should attenuate the effects of external dis- turbances and measurement noise, keeping the system performance relatively unaffected by unexpected inputs. âą Robustness: the controller should perform well despite uncertainties in the sys- tem model or parameters, ensuring reliable operation under varying condi- tions. 92 principles of robot autonomy + k i R t 0 e(Ï)dÏ k p e(t) k d de(t) dt + System Dynamics e(t)u(t) y(t) â r(t) Figure 3.2: Block diagram for a PID controller in a feedback loop.The error signal e(t) is computed as the difference be- tween the reference r(t) and the output y(t). The control input u(t) is generated by combining the proportional, integral, and derivative terms of the error. This feedback principle underlies many widely used strategies in control en- gineering. Among the simplest and most widely adopted strategies arising from this principle is the proportionalâintegralâderivative (PID) controller 2 . Despite its 2 PID control is most naturally for- mulated for single-input, single-output (SISO) linear systems, where the error signal is scalar.Extensions to multi- input multi-output or nonlinear sys- tems are possible, but they often rely on heuristic tuning and provide weaker guarantees. conceptual simplicity, PID has endured the test of time and remains a dominant tool in control engineering, from regulating temperature in industrial furnaces to stabilizing the flight of drones. 3.1.1 Structure of PID Control A PID controller computes the control input as a weighted combination of three terms derived from the tracking error e(t) = r(t)â y(t), where r(t) denotes the desired reference and y(t) the measured output: u(t) = k p e(t) + k i Z t 0 e(Ï)dÏ + k d d dt e(t).(3.3) The three terms serve complementary purposes: âą Proportional (P): reacts immediately to deviations, producing corrective action proportional to the current error. âą Integral (I): accumulates past error, driving steady-state error to zero. âą Derivative (D): predicts future trends by responding to the rate of change, improving transient behavior and damping oscillations. By adjusting the gains (k p , k i , k d ), we can shape how aggressively the controller responds to disturbances, how quickly it eliminates offsets, and how smoothly it approaches the reference trajectory 3 . The structure of a PID controller is depicted 3 Although tuning PID controllers is not trivial, heuristic rules such as the ZieglerâNichols method provide sys- tematic starting points for gain selection, but they often require further refinement in practice. in the block diagram in Section 3.1.1. PID control is ubiquitous in industry pre- cisely because of this simplicity: the controller can be deployed with only limited implementation effort and modest computational resources, which makes it at- tractive for embedded systems. closed-loop control & trajectory tracking 93 Example 3.1.1 (PD control of a double-integrator system). To illustrate the me- chanics of PID-type control, consider a double-integrator system with dynamics: Ìx(t) = " 0 1 0 0 # x(t) + " 0 1 # u(t), where x = [ x 1 , x 2 ] †. Suppose the control objective is to regulate x 1 to the origin. A proportionalâderivative controller of the form: u(t) =âk p e(t)â k d de(t) dt ,e(t) = x 1 (t), yields closed-loop dynamics: Ìx(t) = " 01 âk p âk d # x(t), where we have used the fact that Ì e = Ì x 1 = x 2 . The eigenvalues of this system are: λ =â k d 2 ± 1 2 q k 2 d â 4k p , and, as a result, stability requires selecting k p and k d such that the real parts of these eigenvalues are negative; that is, k p > 0 and k d > 0 4 . By appropriately 4 Note that a proportional controller alone, with k d = 0, will not make the system stable since at best the eigenval- ues would be purely complex, and thus the system response would be an un- damped oscillation. tuning k p and k d , we can trade off speed of convergence against overshoot, os- cillations, and sensitivity to measurement noise. For example, one might set the proportional gain k p to a very large value in order to drive rapid convergence to the origin. In practice, however, excessively large gains tend to amplify mea- surement noise and can cause the controller to behave poorly. For example, with large proportional gain k p , the controller may introduce oscillations, while a large derivative gain k d increases damping but may also slow the systemâs response. Beyond PID and PD control, classical control theory provides a rich suite of tools for analysis and design. These methods are often developed in the frequency domain 5 , where tools such as Bode plots and the Nyquist stability criterion offer 5 Frequency-domain analysis uses the Laplace transform to represent sys- tem dynamics as algebraic relationsâ referred to as transfer functionsârather than differential equations. powerful ways to assess and shape system behavior. For an in-depth treatment of these topics, we refer the reader to Aström and Murray [9]. 3.1.2 Application of PID Control to Tracking Problems Although PID controllers are most commonly associated with regulation tasks, as discussed in Example 3.1.1, the same structure introduced in Section 3.1.1 can also be employed for trajectory tracking, where the objective is to follow a time-varying nominal trajectory. In this setting, the reference signal r(t) evolves over time, and the controller seeks to minimize the instantaneous tracking error e(t) = r(t)â y(t) throughout the motion. While this direct application of the PID framework can perform reasonably well for simple scenarios, such as slowly 94 principles of robot autonomy varying or smooth trajectories, it remains largely heuristic, requiring ad hoc de- sign and tuning of the gains for each specific task and system. As a result, PID- based tracking tends to be fragile and poorly generalizable, with performance degrading even under minor variations to the problem setup. There is, however, an important class of systems where PID-type control can be systematically and effectively applied to tracking: differentially flat systems. As introduced in Section 2.4, differential flatness provides a powerful frame- work for computing open-loop control sequences. Recall that a system with state xâ R n and input uâ R m : Ìx = f (x, u), is said to be differentially flat if there exists a function α such that: z = α(x, u, Ìu, . . . , u (a) ), where u (i) denotes the i-th derivative of u, and such that both the system states and inputs can be expressed as algebraic functions of the flat outputs zâ R m and a finite number of their derivatives: x = ÎČ(z, Ìz, . . . , z (b) ), u = Îł(z, Ìz, . . . , z (c) ). This structural property implies that the full system trajectory is fully determined once the flat outputs z(t) are known. In practice, we can therefore design a trajec- tory directly in the flat-output spaceâusing, for example, spline interpolation or polynomial parameterizationsâand reconstruct the corresponding state and con- trol trajectories algebraically, without the need to integrate the system dynamics. While in Chapter 2 we focused on leveraging differential flatness for open- loop trajectory generation, it is important to highlight how these systems also lend themselves naturally to trajectory tracking. In particular, differential flatness enables the trajectory tracking problem for a nonlinear system to be reduced to a linear tracking problem in the flat-output space, where classic control methodsâ such as PIDâcan be applied. Differentially flat systems possess a particularly useful feature, as they can be feedback linearized to yield a linear system in the flat-output space. Specifically, given a differentially flat system with flat output z = (z 1 , . . . , z m ) there exist a vector of integers r = (r 1 , r 2 , . . . , r m ) such that: x = ÎČ(z 1 , Ì z 1 , . . . , z (r 1 ) 1 , . . . , z m , Ì z m , . . . , z (r m ) m ), u = Îł(z 1 , Ì z 1 , . . . , z (r 1 +1) 1 , . . . , z m , Ì z m , . . . , z (r m +1) m ), (3.4) and such that the system dynamics can be equivalently expressed as a linear system of the form: z (r 1 +1) i = w 1 , z (r 2 +1) 2 = w 2 , . . . z (r m +1) m = w m , (3.5) closed-loop control & trajectory tracking 95 where w = (w 1 , . . . , w m ) is a virtual control input that can be algebraically re- lated to the original system inputs u 6 . The linear system in Equation (3.5) can 6 For a detailed treatment of feedback linearization for differentially flat sys- tems, we refer the reader to Levine [80] and Murray [101]. effectively be controlled using standard linear control techniques, such as PID control. In particular, given a reference flat output trajectory z d = (z d,1 , . . . , z d,m )â computed, for example, by any open-loop methodâand its corresponding virtual input trajectory w d = (w d,1 , . . . , w d,m ), we can define the component-wise tracking error: e i : = z i â z d,i ,i = 1, . . . , m, which implies the following error dynamics: e (r i +1) i = w i â w d,i . To ensure convergence of the tracking error to zero, we can choose the following control law: w i = w d,i â r i â j=0 k i,j e (j) i , which, applied to the system in Equation (3.5), yields the following closed-loop dynamics: z (r i +1) i = w d,i â r i â j=0 k i,j e (j) i . Since w d,i = z (r i +1) d,i by construction, the resulting tracking error dynamics take the form: e (r i +1) i + r i â j=0 k i,j e (j) i = 0, where the gains k i,j > 0 are selected to enforce stability. This procedure effectively reduces the nonlinear trajectory tracking problem to a set of decoupled linear tracking problems, one for each flat output, which can be solved using standard linear control techniques. Below, we illustrate this procedure with a concrete example. Example 3.1.2 (PD control for a dynamically extended unicycle). Consider the dynamically extended unicycle model introduced in Example 1.3.1: Ì x = v cos Ξ, Ì y = v sin Ξ, Ì v = a, Ì Îž = Ï, with state x = [x, y, v, Ξ] †and control input u = [a, Ï] †. This system is differen- tially flat with flat outputs (x, y) and its dynamics can be expressed as: " Ì x Ì y # = " cos Ξ âv sin Ξ sin Ξv cos Ξ # | z J(Ξ,v) " a Ï # | z u : = " w 1 w 2 # ,(3.6) 96 principles of robot autonomy where w = [w 1 , w 2 ] †is a virtual control input representing the flat-output accel- erations, effectively transforming the system into a system of the form in Equa- tion (3.5). As a result, given a reference trajectory (x d , y d ), we can design a feedback law for w that ensures tracking of the desired trajectory. Specifically, we can select the following PD-type virtual control: w 1 = Ì x d + k px (x d â x) + k dx ( Ì x d â Ì x), w 2 = Ì y d + k py (y d â y) + k dy ( Ì y d â Ì y), (3.7) with positive control gains k px , k dx , k py , k dy > 0. Under this law, the Cartesian tracking error obeys a second-order linear differential equation: Ì e x + k dx Ì e x + k px e x = 0, Ì e y + k dy Ì e y + k py e y = 0, ensuring that the tracking error (e x , e y ) = (x d â x, y d â y) converges exponentially to zero. Finally, assuming the Jacobian matrix J(Ξ, v) is invertible, the corresponding control inputs (a, Ï) can be obtained algebraically by: " a Ï # = J â1 (Ξ, v) " w 1 w 2 # . Thus, a conceptually simple PD control law in the flat-output space translates into a nonlinear state feedback law for the original system. 3.2 Optimal Closed-loop Control for Linear Systems Despite their widespread use and practical appeal, classical feedback controllers lack systematic methods to mathematically quantify and optimize performance. As introduced in Chapter 2, modern control theory addresses these issues by formulating controller design as an explicit optimization problem. In this section, we focus on the linearâquadratic setting 7 , which represents one 7 That is, where the system dynamics are linear and the cost function is quadratic. of the most elegant and widely applicable frameworks for closed-loop optimal control. Specifically, we focus on the linear quadratic regulator problem by first presenting its infinite-horizon, continuous-time formulation. Then, we discuss several important extensions of this frameworkâsuch as finite-horizon, discrete- time, and time-varying formulationsâand illustrate how it can be applied to linear tracking problems. 3.2.1 The Linear Quadratic Regulator The linear quadratic regulator (LQR) provides a principled solution to the regula- tion problem, where the primary objective is to drive the state of a linear system to the origin while optimally balancing state deviations against control effort 8 . 8 While this may seem like a restrictive setting at first, we will see in the re- mainder of this chapter that this formu- lation extends naturally to a wide range of practically relevant problems. Formally, consider the linear time-invariant (LTI) system: Ìx(t) = Ax(t) + Bu(t),(3.8) closed-loop control & trajectory tracking 97 where A â R nĂn and B â R nĂm are constant matrices. The objective is to deter- mine a control policy u(t) that minimizes the quadratic cost functional: J(u) = Z â 0 x(t) †Qx(t) + u(t) †Ru(t)dt,(3.9) where Qâ R nĂn is a symmetric positive semidefinite matrix that penalizes devi- ations of the state from the origin and Râ R mĂm is a symmetric positive definite matrix that penalizes control effort. Thus, in its infinite-horizon form, the LQR problem is formalized as the following optimal control problem: minimize u Z â 0 x(t) †Qx(t) + u(t) †Ru(t)dt, subject to Ìx(t) = Ax(t) + Bu(t). (3.10) Assuming that the pair (A, B) is stabilizable and (Q, A) is detectable 9 , the LQR 9 These assumptions guarantee the exis- tence of a unique stabilizing solution to the infinite-horizon LQR problem. For formal definitions and proofs, we refer the reader to Murray [101] and Bertsekas [12]. problem admits a unique optimal solution in the form of a stationary linear state- feedback controller: u(t) =âKx(t),(3.11) where the optimal gain matrix K â R mĂn is given by: K = R â1 B †P.(3.12) Here, Pâ R nĂn is the unique positive semidefinite solution to the continuous-time algebraic Riccati equation: A †P + PAâ PBR â1 B †P + Q = 0.(3.13) The Riccati equation can be derived either through the calculus of variationsâ via the Pontryagin Maximum Principle introduced in Chapter 2âor through dy- namic programming, using the HamiltonâJacobiâBellman equation 10 . Substitut- 10 Both derivations lead to the Riccati equation and the same optimal linear feedback law.We refer the reader to Bertsekas [12] for an in-depth treatment of both approaches. ing the optimal closed-loop policy from Equation (3.11) into the system dynamics from Equation (3.8) yields the closed-loop system: Ìx(t) = (Aâ BK)x(t),(3.14) which is guaranteed to be asymptotically stable, i.e., the eigenvalues of the matrix Aâ BK have strictly negative real parts. Thus, the LQR problem provides a direct recipe for the regulation problem: solve the Riccati equation 11 , compute 11 In practice, solutions to the algebraic Riccati equation are obtained numeri- cally using standardized software pack- ages. Popular implementations include scipy.linalg.solve _ continuous _ are in Python or icare in MATLAB. the optimal gain matrix K, and apply the feedback law u(t) =âKx(t). As a concrete example, Algorithm 8 demonstrates how to compute the opti- mal infinite-horizon LQR controller for a simple double integrator system with dynamics Ì x = u. Designing the cost matrices. A central question in LQR design is how to select the cost matrices Q and R, as these determine the trade-off between state regulation and control effort, thereby shaping the overall closed-loop performance. For a valid solution to exist, the matrices must satisfy Q = Q †âȘ° 0 and R = R †⻠0. 98 principles of robot autonomy Double Integrator Infinite-horizon LQR import numpy as np from scipy.linalg import solve _ continuous _ are # System dynamics matrices for simple double integrator Ì x = u A = np.array([[0, 1], [0, 0]]) B = np.array([[0], [1]]) # Define cost function matrices Q = np.array([[1, 0], [0, 1]]) R = np.array([[1]]) # Solve the continuous algebraic Riccati equation P = solve _ continuous _ are(A, B, Q, R) # Compute optimal feedback gain matrix K = -np.linalg.inv(R) @ B.T @ P # Verify eigenvalues are negative (closed-loop system is stable) eig _ val, eig _ vec = np.linalg.eig(A + B @ K) print(eig _ val) Algorithm 8: Computing the optimal infinite-horizon LQR controller for a simple double integrator with dynamics Ì x = u in Python. The code for this example is available in the repository github.com/StanfordASL/pora-exercises in the notebook ch03/lqr.ipynb. To simplify the discussion, we further assume Q = Q †⻠0. Together with the stabilizability and detectability assumptions stated above, this guarantees the existence of a unique stabilizing solution to the algebraic Riccati equation. In practice, the specific choice of Q and R depends on the designerâs understanding of the system dynamics and performance objectives. A particularly simple and commonly adopted approach is to use diagonal weight matrices, where each diagonal element directly penalizes a corresponding state or control variable: Q = ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° q 1 0 ·0 0q 2 ·0 . . . . . . . . . . . . 00 ·q n ïŁč ïŁș ïŁș ïŁș ïŁș ïŁ» ,R = ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° r 1 0 ·0 0r 2 ·0 . . . . . . . . . . . . 00 ·r m ïŁč ïŁș ïŁș ïŁș ïŁș ïŁ» , where q i > 0 and r j > 0 for all i = 1, . . . , n and j = 1, . . . , m. With this choice, each diagonal entry directly specifies how strongly the corresponding (squared) state or input contributes to the overall cost. In general, states (or equivalently, closed-loop control & trajectory tracking 99 control inputs) that are particularly important to regulate are assigned larger weights, ensuring that deviations in those directions are penalized more heavily. Conversely, less critical states or inputs are assigned smaller weights, reflecting their reduced influence on the systemâs overall performance. Finite-horizon formulation. The LQR formulation can be naturally extended to the finite-horizon setting, where performance is evaluated only over a fixed time interval [0, t f ]. In this case, the optimal control problem can be formulated as: minimize u x(t f ) †Q f x(t f ) + Z t f 0 x(t) †Qx(t) + u(t) †Ru(t)dt, subject to Ìx(t) = Ax(t) + Bu(t), (3.15) where Q f â R nĂn and Q â R nĂn are symmetric positive semidefinite matrices, and R â R mĂm is symmetric positive definite. The matrix Q f specifies the ter- minal cost, penalizing deviations of the state from the origin at the final time t f . As in the infinite-horizon case, the optimal control law retains the linear form u(t) = âK(t)x(t), but the gain matrix becomes time-varying. Specifically, the optimal gain matrix K(t) is given by: K â (t) = R â1 B †P(t),(3.16) where P(t) is the positive semidefinite matrix that solves the continuous-time dif- ferential Riccati equation: Ì P(t) =â A †P(t)â P(t)A + P(t)BR â1 B †P(t)â Q, with terminal condition P(t f ) = Q f . As the horizon length t f tends to infinity, and under the standard assump- tions introduced for the infinite-horizon caseânamely, that (A, B) is stabilizable, (Q, A) is detectable, Q = Q †âȘ° 0, and R = R †⻠0âthe solution P(t) of the differential Riccati equation converges to the steady-state solution P of the alge- braic Riccati equation from Equation (3.13). Correspondingly, the time-varying gain matrix K(t) converges to the constant gain matrix K from Equation (3.12). In practice, for sufficiently long horizons, it is common to use the infinite-horizon gain directly, thereby avoiding the need to compute or store the entire gain sched- ule K(t). Discrete-time LQR. Up to this point, we have focused on continuous-time formu- lations. However, both the finite-horizon and infinite-horizon problems can be posed just as naturally in discrete time, where the system evolves according to the difference equation: x t+1 = Ax t + Bu t ,(3.17) and where, as discussed in Chapter 2, the time horizon t f is discretized into N â N intervals t = 0, 1, . . . , Nâ 1 of lengthât = t f /N. In this setting, the 100 principles of robot autonomy structure of the optimal solution mirrors the continuous-time case, where the optimal state-feedback law is again linear in the state, u t = âK t x t , with gains obtained by solving a Riccati equationânow in its discrete form. Specifically, the finite-horizon problem in discrete time is formulated as: minimize u x †N Q f x N + Nâ1 â t=0 x †t Qx t + u †t Ru t , subject to x t+1 = Ax t + Bu t ,t = 0, . . . , Nâ 1, (3.18) where Q f = Q †f âȘ° 0, Q = Q †âȘ° 0, and R = R †⻠0. The optimal solution is obtained through a backward Riccati recursion, which proceeds from the terminal condition P N = Q f and iterates backward in time for t = Nâ 1, Nâ 2, . . . , 0 according to: P t = Q + A †P t+1 Aâ A †P t+1 B R + B †P t+1 B â1 B †P t+1 A,(3.19) with the corresponding feedback gain: K t = R + B †P t+1 B â1 B †P t+1 A.(3.20) This recursion can be viewed as the discrete-time analogue of integrating the continuous-time differential Riccati equation backward from t f to 0. At each step, P t is updated one stage earlier, effectively propagating future cost information back through time to determine the optimal closed-loop policy. In the infinite-horizon setting, the problem is formulated as: minimize u â â t=0 x †t Qx t + u †t Ru t , subject to x t+1 = Ax t + Bu t , (3.21) where Q = Q †âȘ° 0 and R = R †⻠0. Analogous to the continuous-time case, and under the standard stabilizability and detectability conditions, the backward Ric- cati recursion convergesâas the horizon length t f tends to infinityâto a steady- state matrix P, which satisfies the discrete-time algebraic Riccati equation: P = Q + A †PAâ A †PB(R + B †PB) â1 B †PA.(3.22) The corresponding optimal feedback gain is then time-invariant and given by: K = (R + B †PB) â1 B †PA.(3.23) Thus, while the overall structure of the LQR solution carries over seamlessly to discrete time, the computational machinery changes, where instead of solving a continuous-time differential or algebraic Riccati equation, one either propagates a backward recursion over a finite horizon (finite case) or solves a fixed-point equation (infinite case). This recursive viewpoint is also closely aligned with the principles of dynamic programming, which is a foundational concept in optimal closed-loop control & trajectory tracking 101 control and reinforcement learning and will be discussed in more detail in Chap- ter 17. Below, we summarize key extensions of the LQR problem using its discrete- time formulation. Time-varying LQR with cross-quadratic costs. We now consider the finite-horizon, discrete-time LQR problem with both time-varying dynamics and cross-quadratic cost terms: minimize u 1 2 x †N Q f x N + Nâ1 â t=0 1 2 x †t Q t x t + 1 2 u †t R t u t + x †t S t u t , subject to x t+1 = A t x t + B t u t ,t = 0, . . . , Nâ 1, (3.24) where Q f = Q †f âȘ° 0, and where Q t = Q †t âȘ° 0, R t = R †t â» 0, and S t are time-varying cost matrices. The system matrices A t and B t are also explicitly time-dependent, capturing nonstationary dynamics. This formulation extends the standard LQR problem by incorporating the cross-term matrices S t , which penalize stateâcontrol interactions. Such terms naturally arise when the cost of applying a control action depends on the current system state. For example, in a vehicle control scenario, the cost of applying a braking force may vary with the vehicleâs speed. The optimal solution retains the linear feedback form u t =âK t x t , with: K t = (R t + B †t P t+1 B t ) â1 (B †t P t+1 A t + S †t ),(3.25) and where the matrices P t are obtained from the backward Riccati recursion: P t = Q t + A †t P t+1 A t â (A †t P t+1 B t + S t )(R t + B †t P t+1 B t ) â1 (B †t P t+1 A t + S †t ), (3.26) with terminal condition P t f = Q f . LQR with affine dynamics and quadratic and linear costs. We now consider a more general discrete-time LQR formulation that incorporates affine dynamics together with quadratic, linear, and constant terms in the cost. Let Q f = Q †f âȘ° 0, Q t = Q †t âȘ° 0, and R t = R †t â» 0. The cost function is given by: J(u) = h(x N ) + Nâ1 â t=0 g(x t , u t ), h(x N ) = 1 2 x †N Q f x N + q †f x N + α f , g(x t , u t ) = 1 2 x †t Q t x t + 1 2 u †t R t u t + x †t S t u t + + q †t x t + r †t u t + α t ,t = 0, . . . , Nâ 1, (3.27) subject to the affine dynamics: x t+1 = A t x t + B t u t + c t .(3.28) 102 principles of robot autonomy The optimal control policy can be derived using dynamic programming, yielding a time-varying affine closed-loop control law 12 . To streamline notation, let us 12 In this section, we cite the result with- out proof and refer the reader to Bert- sekas [12] for a detailed derivation of the optimal solution for various LQR exten- sions. define the following intermediate quantities: η t : = α t + ÎČ t+1 + p †t+1 c t + 1 2 c †t P t+1 c t , h x,t : = q t + A †t ( p t+1 + P t+1 c t ) , h u,t : = r t + B †t ( p t+1 + P t+1 c t ) , H x,t : = Q t + A †t P t+1 A t , H xu,t : = S t + A †t P t+1 B t , H u,t : = R t + B †t P t+1 B t . (3.29) The optimal law remains affine in the state: u t =âK t x t â k t ,(3.30) with parameters given recursively by: P N = Q f , p N = q f , ÎČ N = α f , K â t : = H â1 u,t H †xu,t , k t : = H â1 u,t h u,t , P t : = H x,t â H xu,t K â t , p t : = h x,t â H xu,t k t , ÎČ t : = η t â 1 2 h †u,t k t . (3.31) This affineâquadratic formulation generalizes the classical LQR by accommodat- ing affine dynamics and nonhomogenous cost terms. As we will see in the re- mainder of this chapter, this formulation plays a central role in extending LQR techniques to nonlinear systems, where such affine and cross-linear structures naturally arise through linearization and quadratic approximation of the dynam- ics and cost functions. In summary, the LQR framework represents a powerful tool for closed-loop optimal control of linear systems with quadratic costs. As we will see next, the principles underlying LQR can be extended to tackle more complex scenarios, including trajectory tracking and nonlinear dynamics. 3.2.2 Linear Tracking Problems As discussed in the previous section, the LQR framework provides a principled solution to the regulation problem, in which the goal is to drive the system state to the origin. In many practical settings, however, tasks may extend well beyond reg- ulation. Robotic manipulators must follow preplanned motions, autonomous ve- hicles must pass through waypoints, and industrial systems often operate around closed-loop control & trajectory tracking 103 time-varying setpoints. In such scenarios, the control objective is one of trajec- tory tracking rather than regulation. Thus, restricting control design to regulation about the origin is often insufficient for many real-world applications. The tracking problem can be naturally formulated within the two-step design paradigm introduced earlier in Equation (3.2). In the first step, an open-loop opti- mal control problem is solved to obtain a nominal stateâcontrol trajectory: ( Ìx(t), Ìu(t) ) ,tâ [0, t f ], that satisfies the system dynamics and optimizes a chosen performance crite- rion 13 . In the second step, a closed-loop controller is designed to ensure that the 13 These nominal trajectories are typi- cally computed using trajectory opti- mization methods, as discussed in Chap- ter 2. actual system trajectory remains close to this nominal trajectory, compensating for disturbances, modeling errors, and measurement noise. Formally, consider the linear system given by: Ìx(t) = Ax(t) + Bu(t),(3.32) and let ( Ìx(t), Ìu(t)) denote a nominal trajectory we wish to track, which satisfies the same dynamics, that is: Ì Ìx(t) = A Ìx(t) + B Ìu(t).(3.33) We define the deviation (or error) variables as: ÎŽx(t) = x(t)â Ìx(t),ÎŽu(t) = u(t)â Ìu(t),(3.34) which represent the difference between the actual state and control inputs and their nominal counterparts. By using the definition of ÎŽx(t) in Equation (3.34) and substituting the dynamics from Equations (3.32) and (3.33), we can derive the dynamics of the deviation variables: ÎŽ Ìx(t) = Ìx(t)â Ì Ìx(t) = [ Ax(t) + Bu(t)]â [A Ìx(t) + B Ìu(t)] = A(x(t)â Ìx(t)) + B(u(t)â Ìu(t)) = AÎŽx(t) + BÎŽu(t), which have the same linear structure as the original system, only now expressed in terms of the deviation variables. This observation is central, as it allows the direct application of LQR techniques to the tracking problem. LQR formulation for tracking. Tracking performance can be naturally expressed through a quadratic cost on the deviation variables: J(ÎŽu(t)) = ÎŽx(t f ) †Q f ÎŽx(t f ) + Z t f 0 ÎŽx(t) †QÎŽx(t) + ÎŽu(t) †RÎŽu(t) dt,(3.35) where QâȘ° 0, Râ» 0, and Q f âȘ° 0 are weighting matrices that penalize deviations of the state and control from their nominal values. The tracking problem can thus 104 principles of robot autonomy be expressed as the finite-horizon optimal control problem: minimize ÎŽu ÎŽx(t f ) †Q f ÎŽx(t f ) + Z t f 0 ÎŽx(t) †QÎŽx(t) + ÎŽu(t) †RÎŽu(t) dt, subject to ÎŽ Ìx(t) = AÎŽx(t) + BÎŽu(t), (3.36) which is precisely the finite-horizon LQR problem introduced in Section 3.2.1, but now formulated in terms of the deviation variables. Therefore, all the results derived for finite-horizon LQR apply directly to this tracking formulation, and the optimal control law takes the form: ÎŽu(t) =âK(t)ÎŽx(t),(3.37) where K(t) is the time-varying optimal feedback gain obtained by solving the differential Riccati equation from Equation (3.16). Finally, substituting the definitions of the deviation variables from Equation (3.34) yields the optimal control law in terms of the original state and control variables: u(t) = Ìu(t)â K(t) x(t)â Ìx(t) .(3.38) This expression is a clear instantiation of the two-step design paradigm, where the control input consists of a feedforward term Ìu(t), derived from the nominal trajectory, and a feedback term âK(t)(x(t)â Ìx(t)) that actively compensates for any deviations of the actual state from the nominal trajectory. 3.3 Optimal Closed-loop Control for Nonlinear Systems The previous section focused on linear systems, for which the LQR framework provides elegant closed-form solutions. However, many real-world systems are inherently nonlinear, making the extension of optimal closed-loop control to non- linear dynamics crucial for numerous practical applications. In this section, we first briefly contextualize dynamic programming and the HJB equationâtwo foundational methods for deriving globally optimal closed- loop control laws in Section 3.3.1, referring the reader to Chapter 18 and Bertsekas [12] for a more in-depth discussion. We then turn our attention to approaches that aim to leverage the structure and insights from linear optimal control to solve nonlinear optimal control problems. For trajectory-tracking, we begin by showing how the notion of linearization enables the application of LQR tracking techniques to nonlinear systems in Section 3.3.2. Finally, we extend ideas from linearization and LQR to develop algorithms that simultaneously generate an open-loop trajec- tory and closed-loop tracking controller in two-step design form. Specifically, we present iLQR and DDP in Section 3.3.3. 3.3.1 Dynamic Programming and the HamiltonâJacobiâBellman Equation When deriving optimal closed-loop control policies for nonlinear systems, one typically distinguishes between two complementary formulations: discrete-time and continuous-time. closed-loop control & trajectory tracking 105 In the discrete-time setting, the primary tool is dynamic programming, orig- inally developed by Richard Bellman in the 1950s. Dynamic programming pro- vides a systematic framework for solving optimal control problems with addi- tive cost functions and nonlinear dynamics, and it underlies numerous clas- sical and modern optimal control algorithms, including many learning-based approaches 14 . Intuitively, dynamic programming decomposes the optimization 14 For instance, reinforcement learning algorithms discussed in Chapter 18. problem into a sequence of smaller subproblems that can be solved recursively by exploiting the principle of optimality, which informally states that for a sequence of optimal decisions, the tail of the optimal sequence is also optimal for a tail sub- problem. This recursive structure leads to the celebrated Bellman equation, which dramatically simplifies the search for optimal policies by transforming the global optimization problem into a tractable sequence of smaller optimization problems. In the continuous-time setting, the analogous formulation is given by the HJB equation. The HJB equation is a nonlinear partial differential equation whose solution enables the derivation of an optimal control law. Conceptually, it cap- tures the infinitesimal version of the dynamic programming principle, providing a continuous-time characterization of optimality. Both the discrete-time dynamic programming paradigm and the continuous- time HJB equation offer powerful, theoretically grounded approaches for deriving globally optimal closed-loop policies for nonlinear systems. However, they also suffer from severe computational challenges, such as the curse of dimensionality, which limit their direct applicability to high-dimensional systems. Practical methods inspired by dynamic programming principles will be ex- plored further in Chapter 18, while for an in-depth discussion of the HJB frame- work, we refer the reader to Bertsekas [12]. 3.3.2 Linear Methods for Nonlinear Tracking Control A common strategy for controlling nonlinear systems is to approximate their dy- namics locally by a linear system and then apply linear control techniques, such as LQR tracking from Section 3.2.2. The process of approximating a nonlinear system by a linear one is called linearization and it leverages the fact that any smooth nonlinear function can be approximated locally by its first-order Taylor expansion. Below, we first describe the linearization process and then show how to apply LQR tracking to the resulting linearized system. Linearization of nonlinear systems. Consider the nonlinear system dynamics: Ìx(t) = f (x(t), u(t)),(3.39) where f : R n Ă R m â R n is a smooth nonlinear function that governs the sys- temâs evolution. Let ( Ìx(t), Ìu(t) ) denote a nominal state and control trajectory that satisfies the dynamics, that is: Ì Ìx(t) = f ( Ìx(t), Ìu(t)).(3.40) 106 principles of robot autonomy For the purposes of trajectory-tracking, we are interested in controlling the sys- tem so that it remains close to this nominal trajectory, i.e., such that the state deviations ÎŽx(t) = x(t)â Ìx(t) are small. As in the linear case (see Section 3.2.2), let us first derive the dynamics of these deviation variables and then design a controller to stabilize them around zero. Using Equations (3.39) and (3.40), the deviation dynamics can be written as: ÎŽ Ìx(t) = Ìx(t)â Ì Ìx(t) = f (x(t), u(t))â f ( Ìx(t), Ìu(t)),(3.41) where, to approximate the right-hand side, we can linearize the nonlinear func- tion f around the nominal trajectory. Specifically, assuming that the deviations ÎŽx(t) and ÎŽu(t) = u(t)â Ìu(t) are small, a first-order Taylor expansion of f yields: f (x(t), u(t))â f ( Ìx(t), Ìu(t)) + â f âx ( Ìx(t), Ìu(t)) |z : = A(t) ÎŽx(t) + â f âu ( Ìx(t), Ìu(t)) |z : =B(t) ÎŽu(t), (3.42) where A(t) â R nĂn and B(t) â R nĂm are the Jacobians of f with respect to the state and control input, evaluated along the nominal trajectory, that is, at ( Ìx(t), Ìu(t)). Substituting the approximation from Equation (3.42) into Equation (3.41), we obtain: ÎŽ Ìx(t)â f ( Ìx(t), Ìu(t)) + A(t) ÎŽx(t) + B(t) ÎŽu(t) â f ( Ìx(t), Ìu(t)), (3.43) which yields the linearized dynamics for the deviation variables: ÎŽ Ìx(t)â A(t)ÎŽx(t) + B(t)ÎŽu(t).(3.44) Applying LQR tracking to the linearized system. Once in this form, standard lin- ear control techniques, such as the LQR tracking framework, can be directly applied to design a feedback controller that stabilizes the deviation dynamics around zero. For instance, applying the finite-horizon LQR tracking framework from Section 3.2.2 yields the optimal feedback law: ÎŽu(t) =âK(t) ÎŽx(t). which, expressed in the original variables, gives the closed-loop controller: u(t) = Ìu(t)â K(t) x(t)â Ìx(t) , where K(t) is the time-varying feedback gain computed by solving the differential Riccati equation associated with the linearized system. Linearizing around an equilibrium point. A particularly important special case arises when the nominal trajectory corresponds to a constant equilibrium ( Ìx, Ìu). A state Ìx is referred to as an equilibrium if there exists a control input Ìu (called the closed-loop control & trajectory tracking 107 equilibrium input) such that f ( Ìx, Ìu) = 0. In other words, if the system is initial- ized at Ìx and the constant input Ìu is applied for all t â„ t 0 , the state will remain fixed at Ìx indefinitely. Linearizing about ( Ìx, Ìu) produces a system of the form in Equation (3.44), where A and B would be constant matrices (not time-varying) evaluated at the equilibrium. Local validity of linearization. It is important to highlight that linearization pro- vides only a local approximation, and the linearized model accurately captures the system dynamics only in a neighborhood of the nominal trajectory. If the state deviates significantly from Ìx(t), the linear approximation may no longer hold, and the resulting controller may perform poorly or even destabilize the system. Below, we illustrate the linearization process with a concrete example. Figure 3.3: An inverted pendu- lum consisting of a point mass m attached to a rigid rod of length l. The motion is described by the angle Ξ from the upright vertical, and u denotes the control torque applied about the pivot. Example 3.3.1 (Inverted pendulum). Consider the inverted pendulum shown in Figure 3.3. Its nonlinear dynamics are given by: ml 2 Ì Îž = mgl sin(Ξ) + u, where m denotes the mass, l is the length of the rod, g is the acceleration due to gravity, Ξ is the pendulum angle, and u is the control torque. Introducing the state vector x : = Ξ Ì Îž †, the system can be written in state-space form as: Ìx = f (x, u) = " Ì Îž g l sin(Ξ) + 1 ml 2 u # . The upright stationary position Ì x = [ 0, 0 ] †is an equilibrium point for this system with equilibrium control input Ì u = 0. Linearizing about this equilibriumâone where ÎŽx = xâ Ìx = x and ÎŽu = uâ Ì u = uâyields the linear model: ÎŽ Ìx = " 01 g l 0 # ÎŽx + " 0 1 ml 2 # ÎŽu, whose state matrix has one eigenvalue with positive real part, and is therefore unstable. To stabilize the pendulum, we can design a proportionalâderivative controller of the form: ÎŽu(t) =âk p Ξ(t)â k d Ì Îž(t), which would yield the closed-loop dynamics: ÎŽ Ìx = " 01 g l â 1 ml 2 k p â 1 ml 2 k d # ÎŽx. By selecting suitable gains k p > 0 and k d > 0, we can ensure that both eigenval- ues of the closed-loop system have negative real parts, ensuring stability around the upright positionâanalogously to the double-integrator PD design in Exam- ple 3.1.1. Of course, PD control is only one option, and other methodsâsuch as the LQRâcan also be applied to the linearized model. 108 principles of robot autonomy Algorithm 9 shows a Python implementation of the linearization process for the inverted pendulum example using JAX, a library for high-performance nu- merical computing and automatic differentiation. Inverted Pendulum Dynamics Linearization import jax import jax.numpy as jnp def inverted _ pendulum _ dynamics(x, u, g=9.81, m=1, l=1): """ Evaluate the inverted pendulum dynamics. """ Ξ, dΞ _ dt = x dx _ dt = jnp.array([dΞ _ dt, (g/l) * jnp.sin(Ξ) + (1/m * l ** 2) * u]) return dx _ dt # Linearize around the stationary upright position with zero # control (i.e. the pendulum is perfectly balanced) f _ jac = jax.jacobian(inverted _ pendulum _ dynamics, argnums=(0, 1)) x = jnp.array([0., 0.]) u = 0. A, B = f _ jac(x, u) # Evaluate Jacobian at equilibrium point Algorithm 9: Linearizing the inverted pendulum dynamics from Example 3.3.1 in Python using the JAX library. The code for this example is available in the repository github.com/StanfordASL/pora-exercises in the notebook ch03/jax _ linearization.ipynb. 3.3.3 Iterative LQR (iLQR) and Differential Dynamic Programming (DDP) In the previous sections, we saw how the LQR frameworkâalong with its var- ious extensionsâprovides a principled foundation for regulation and tracking, and how the same ideas can be applied to both linear and nonlinear systems. Specifically, we highlighted how LQR can be naturally embedded within a two- step design paradigm, serving as a practical alternative to directly solving the full nonlinear optimal control problem by combining elements of open-loop trajectory generation and closed-loop tracking control. In this section, we adopt a different yet complementary perspective, and ex- plore how ideas from linearization and LQR can be extended to develop algo- rithms that simultaneously generate an open-loop trajectory and a closed-loop tracking controller in two-step design form. This approach leads to two closely related algorithms: the iterative linear quadratic regulator (iLQR) and differential dy- namic programming (DDP). As we will see, the key insight underlying both meth- closed-loop control & trajectory tracking 109 ods is that the structure which makes LQR tractableânamely, quadratic cost and linear dynamicsâcan be embedded within an iterative optimization scheme to efficiently handle nonlinear dynamics and non-quadratic costs. Iterative LQR. Recall from the LQR tracking problem in Section 3.2.2 that the controller acts to drive deviation variables (ÎŽx, ÎŽu) to zero, thereby ensuring the system follows a specified reference. At its core, iLQR uses the same machinery, but with a different objective: instead of driving the deviation variables to zero, the optimal deviations are used to modify the nominal trajectory itself. By repeating this process, iLQR gradually refines the trajectory until no further improvement can be made, yielding a locally optimal solution. Formally, consider the discrete-time finite-horizon nonlinear optimal control problem: minimize u h(x N ) + Nâ1 â t=0 g(x t , u t ), subject to x t+1 = f (x t , u t ),t = 0, . . . , Nâ 1, (3.45) Given a feasible nominal trajectory ( Ì x 0 , Ì u 0 , . . . , Ì x N , Ì u N ), let us linearize the dy- namics and quadratize the cost around the nominal trajectory as: ÎŽx t+1 â â f âx ( Ì x t , Ì u t ) |z : =A t ÎŽx t + â f âu ( Ì x t , Ì u t ) |z : =B t ÎŽu t + 0 |z : =c t , h(x N )â h ( Ì x N ) |z : =α f +âh ( Ì x N ) | z : =q f ÎŽx N + 1 2 ÎŽx N â 2 h ( Ì x N ) | z : =Q f ÎŽx N , g t (x t , u t )â g t ( Ì x t , Ì u t ) |z : =α t +â x g ( Ì x t , Ì u t ) | z : =q t ÎŽx t +â u g ( Ì x t , Ì u t ) | z : =r t ÎŽu t + 1 2 ÎŽx †t â 2 x g t ( Ì x t , Ì u t ) | z : =Q t ÎŽx t + 1 2 ÎŽu †t â 2 u g t ( Ì x t , Ì u t ) | z : =R t ÎŽu t + ÎŽx †t â 2 xu g t ( Ì x t , Ì u t ) | z : =S t ÎŽu t , (3.46) where Q, R, S are Hessians and q, r the gradients of the cost with respect to state and input, evaluated along ( Ìx, Ìu). This reduces the problem to the LQR formula- tion introduced in Equation (3.27), which can be solved via the Riccati equations to obtain the optimal deviations (ÎŽx â , ÎŽu â ). iLQR alternates between two complementary steps: âą Backward pass: linearize the dynamics and quadratize the cost around ( Ìx, Ìu). Solve the resulting LQR problem to obtain the affine control law for the devi- ation variables: ÎŽu â t =âK t ÎŽx t â k t .(3.47) 110 principles of robot autonomy âą Forward pass: starting from the initial condition x 0 , propagate the nonlinear system forward using: u t = Ìu t â k t â K t (x t â Ìx t ),(3.48) and update the nominal trajectory to ( Ìx, Ìu) = (x, u). These steps are repeated until the trajectory converges, typically measured by negligible improvement in cost or small changes in the control sequence. Practical considerations. While iLQR provides a powerful framework for trajec- tory optimization, its performance depends critically on several implementation details: âą Local optimality. iLQR produces solutions that are only locally optimal. The resulting open-loop trajectory ( Ìx, Ìu) minimizes the cost only in a neighborhood of the initial guess, and the associated feedback law stabilizes the system only locally. Consequently, a good initialization of the nominal trajectory is often critical to success. âą Second-order terms. The quadratic expansion of the cost introduces second- order terms H x,t and H u,t , which in general may not be positive semidefinite and positive definite, respectively. To ensure well-posed Riccati recursions, these terms are often regularized to be invertible, for example by adding a multiple of the identity matrix (H x,t + ÎŒ I, H u,t + ÎŒ I), with ÎŒ > 0, or by projecting them to the nearest valid matrices. âą Termination criteria. Since iLQR is iterative, a stopping rule must be defined. In practice, iterations are terminated either when the change in the control trajec- tory is sufficiently small, or when the improvement in cost between successive iterations falls below a chosen threshold. âą Forward pass robustness. During the forward pass, care must be taken to ensure that the updated trajectory does not deviate too far from the one used in the linearization. Common strategies include penalizing large deviations more heavily or performing a line search on the step size used to update the controls and states. These considerations are critical for achieving robust and efficient performance in practice. A comprehensive collection of tips and detailed mathematical treatment of iLQR can be found in Tassa [146]. Differential dynamic programming. Closely related to iLQR is DDP, an algorithm rooted in both LQR and dynamic programming 15 . Like iLQR, DDP alternates be- 15 Introduced in Section 3.3.1 and which will be further discussed in Chapter 17. tween a backward pass (computing locally optimal gains) and a forward rollout (updating the trajectory). The key difference lies in the treatment of the backward pass. In iLQR, the dynamics are linearized to first order, so the resulting formu- lation involves second-order terms only from the cost function. In contrast, DDP closed-loop control & trajectory tracking 111 also expands the dynamics to second order, so that curvature information from the nonlinear dynamics explicitly enters the Riccati recursion. Formally, compared to the solution introduced in Equation (3.29), DDP aug- ments the matrices H x,t , H u,t , and H xu,t with additional second-order terms, as follows: H x,t : = Q t + A †t P t+1 A t + n â i=1 p t+1,i â 2 x f i ( Ìx t , Ìu t ), H u,t : = R t + B †t P t+1 B t + n â i=1 p t+1,i â 2 u f i ( Ìx t , Ìu t ), H xu,t : = S t + A †t P t+1 B t + n â i=1 p t+1,i â 2 xu f i ( Ìx t , Ìu t ). (3.49) In practice, this richer approximation often improves convergence behavior and solution accuracy, especially in strongly nonlinear systems. However, it also makes DDP more computationally expensive than iLQR, since it requires eval- uating and storing second-order derivatives of the dynamics at every iteration. 3.4 Model Predictive Control (MPC) We now turn to Model Predictive Control (MPC), also known as Receding Horizon Control (RHC), which has become one of the most influential methodologies in modern control, both in theory and in practice. To appreciate its role, it is helpful to revisit the distinction between open-loop and closed-loop control. In an open-loop formulation, the control problem is posed as finding a tra- jectory u(t) that optimizes a given performance criterion subject to system con- straints. While conceptually straightforward and computationally cheaper, this approach does not account for disturbances or deviations from the nominal tra- jectory that may arise during execution. By contrast, closed-loop control directly maps the measured state to a control action through a feedback policy. This design enables the control input to adapt to the current system state. However, computing optimal feedback policies can quickly become intractable, even for systems of moderate size 16 . 16 Earlier, we introduced PID controllers and LQR as computationally efficient ex- amples of closed-loop control.These methods, however, are restricted to rela- tively simple settings such as linear sys- tems with quadratic costs. More gen- eral cases often quickly become compu- tationally prohibitive. At a high level, MPC can be seen as a principled unification of these two paradigms, using repeated open-loop optimization to effectively achieve closed- loop behavior. At each time step, MPC solves a finite-horizon open-loop optimal control problem, yielding an input sequence u 0 , u 1 , . . . , u Nâ1 . Only the first con- trol action u 0 is applied, after which the horizon shifts forward, the state is re- measured, and the optimization is solved again. This receding-horizon strategy effectively embeds feedback into open-loop optimization, combining the robust- ness of closed-loop adaptation with the computational tractability of open-loop planning. Naturally, MPC requires additional design elements to ensure reliable oper- ation. Two key concerns are persistent feasibility (the guarantee that a valid so- lution can be found at each step) and stability (ensuring the closed-loop system 112 principles of robot autonomy Figure 3.4: The receding hori- zon principle.At each time step, a finite-horizon optimal control problem is solved based on the current state measure- ment. Only the first control in- put is applied, and the process repeats at the next time step. behaves well over time). These are typically addressed through terminal con- straints, tailored terminal costs, or other problem-specific design choices, which we will return to later in this section. Historically, MPC emerged in the 1970s within the process control community, particularly in chemical engineering. This application domain was ideal for two reasons. First, safety-critical constraints (e.g., temperature limits) could be en- forced explicitly within the optimization problem. Second, chemical processes evolve on relatively slow timescales, giving ample time to solve optimization problems onlineâeven with the limited computational power of the era. With todayâs hardware, these limitations have largely disappeared, and MPC is now deployed in real-time domains such as robotics, where numerical optimization problems may be solved at tens or hundreds of Hertz. Today, MPC is considered one of the cornerstones of modern control. Along- side PID control and LQR, it is among the most widely applied methodologies, valued for its ability to explicitly handle constraints while maintaining robust- ness. The material in this section provides the foundations necessary to under- stand and implement MPC. For readers interested in deeper coverage of both theory and practice, we recommend the textbooks by Borrelli, Bemporad, and Morari [19] and Rawlings, Mayne, and Diehl [120]. closed-loop control & trajectory tracking 113 3.4.1 The Receding Horizon Framework The underlying setting for MPC is the discrete-time infinite-horizon optimal con- trol problem. That is, in principle, we would like to design a feedback policy that minimizes a cost accumulated over an infinite time horizon, that is, with final time t f = +â. Solving for closed-loop policies in this setting is, however, typically intractable. MPC addresses this challenge through a suboptimalâbut computationally tractableâapproximation. Essentially, instead of solving the infinite-horizon prob- lem directly, it repeatedly solves a finite-horizon optimal control problem. The idea is simple and illustrated in Section 3.4.1. At the current time t, the system state is measured, and an optimal control sequence is computed over a horizon of length N. As in any open-loop formulation, the optimization relies on the system model to predict how the state will evolve under candidate input sequences, se- lecting the one that minimizes the cost over the finite horizon. This process yields a sequence of control inputs: u t , u t+1 , . . . , u t+Nâ1 , along with the corresponding predicted state trajectory. Crucially, only the first control input u t is applied to the system. Once time advances to t + 1, the re- maining inputs u t+1 , . . . , u t+Nâ1 are discarded, and a new optimization problem is solved based on the updated state measurement. This process is then repeated recursively. This scheme is often referred to as the receding horizon framework, where the horizon âmoves forwardâ with time, and at each step a new optimization problem is solved based on the latest state information. While it may appear wasteful to discard the unused portion of the control trajectory, this is precisely what allows MPC to incorporate updated measurements and disturbances, ensuring feedback is embedded into the process. If the finite-horizon problems are designed and tuned carefullyâthrough ap- propriate horizon lengths, terminal costs, or constraintsâthe closed-loop behav- ior of MPC can closely approximate that of the true infinite-horizon optimal con- troller. In practice, MPC often achieves performance nearly indistinguishable from the infinite-horizon optimal solution, while remaining computationally fea- sible. 3.4.2 Basic Formulation To formalize the ideas introduced above, consider the discrete-time linear time- invariant (LTI) system: x t+1 = Ax t + Bu t ,(3.50) where x t â R n and u t â R m denote the state and input vectors, respectively. Both are subject to constraints of the form: x t âX , u t âU , âtâ„ 0,(3.51) 114 principles of robot autonomy where X â R n and U â R m are convex polyhedral sets describing admissible states and inputs. At time t, given the measured state x t , MPC computes an input sequence over a prediction horizon of length N: u t|t , u t+1|t , . . . , u t+Nâ1|t , where the notation u t+k|t indicates âthe input applied at time t + k as predicted at time tâ. Correspondingly, we denote the predicted state trajectory as: x t|t , x t+1|t , . . . , x t+N|t ,with x t|t = x t . Finite-horizon problem. The control inputs are obtained by solving the finite- horizon optimal control problem: J â t (x t ) = minimize U tât+N|t h(x t+N|t ) + Nâ1 â k=0 g(x t+k|t , u t+k|t ), subject to x t+k+1|t = Ax t+k|t + Bu t+k|t ,k = 0, . . . , Nâ 1, x t+k|t âX , u t+k|t âU ,k = 0, . . . , Nâ 1, x t+N|t âX f , x t|t = x t , (3.52) where U tât+N|t denotes the sequence of decision variables u t|t , . . . , u t+Nâ1|t , X f is a terminal set, and the functions g(·,·) and h(·) represent stage and terminal costs. Control law. Only the first element of the optimal input sequence is applied to the system: u t = u â t|t (x t ),(3.53) where the dependency on x t arises from the fact that the optimization problem in Equation (3.52) has the condition x t|t = x t . At the next step, t + 1, the state is re-measured, and Problem (3.52) is solved again with updated information. This iterative procedure defines the receding horizon control law, embedding feedback into the overall process. Closed-loop dynamics. The resulting closed-loop system can be written compactly as: x t+1 = Ax t + BÏ t (x t ) = f cl (x t ),tâ„ 0,(3.54) where Ï t (x t ) : = u â t|t (x t ) is the MPC feedback law at time t. Because the dynamics, cost, and constraints are time-invariant, the problem can be equivalently written by fixing the initial time t = 0. Formally, let x 0 = x t and U 0 = u 0 , . . . , u Nâ1 , we closed-loop control & trajectory tracking 115 can rewrite the problem in Equation (3.52) as: J â 0 (x t ) = minimize U 0 h(x N ) + Nâ1 â k=0 g(x k , u k ), subject to x k+1 = Ax k + Bu k ,k = 0, . . . , Nâ 1, x k âX , u k âU ,k = 0, . . . , Nâ 1, x N âX f , x 0 = x t . (3.55) This formulation is often more convenient for analysis, since it avoids carrying the explicit time index through the condition x 0 = x t . Typical cost functions. In practice, MPC problems are often defined by quadratic stage and terminal costs of the form: h(x N ) = x †N Q f x N ,g(x k , u k ) = x †k Qx k + u †k Ru k ,(3.56) with Q f âȘ° 0, QâȘ° 0, and Râ» 0. This leads to the overall cost function: J 0 (x 0 ) = x †N Q f x N + Nâ1 â k=0 x †k Qx k + u †k Ru k .(3.57) Quadratic costs are not only natural but also lead to convex optimization prob- lems when paired with polyhedral constraints. Alternative norms, such as â 1 or â â , are sometimes used to promote robustness or sparsity in the control actions 17 . 17 For instance, â 1 penalties are com- monly used in predictive control formu- lations for systems with actuator limita- tions or to induce sparse actuation. Implementation challenges. While MPC provides a powerful framework, it also raises two critical implementation challenges: 1. Persistent feasibility. Even if the optimization problem is feasible at the current time step, there is no guarantee that feasibility will be preserved in the future, as the chosen inputs may drive the system into a region of the state space from which no admissible solution exists. This situation arises when the controller fails to âlook aheadâ sufficiently to maintain long-term feasibility. For example, consider an autonomous car driving towards an obstacle at high speed, and the MPC controller chooses to brake too late because of a short-sighted horizon. As a result, the car may end up in a state where it cannot stop in time to avoid a collision, leading to infeasibility in subsequent control steps. 2. Closed-loop stability. Feasibility alone does not guarantee that the closed-loop trajectories converge to the origin. In fact, it is possible for the MPC law to produce admissible control inputs that indefinitely satisfy the constraints but fail to stabilize the system. These issues highlight a fundamental tension: MPC is based on solving short- horizon problems, yet we ultimately require effective long-term behavior. How can a strategy that looks only N steps ahead ensure both feasibility and stability over a potentially infinite horizon? 116 principles of robot autonomy A central insight is that these properties can be enforced through appropriate design of the terminal cost and terminal constraint setâdenoted as h(·) and X f in (3.52), respectively. Roughly speaking, the terminal cost serves as a surrogate for the infinite-horizon tail of the problem, while the terminal set ensures that the system remains within a region where the behavior is well-understood and manageable. Together, these design choices represent a flexible tool to reconcile the short-sighted nature of MPC with the long-term guarantees we ultimately care about. While a full treatment of these issues is beyond the scope of this book, we refer the interested reader to Borrelli, Bemporad, and Morari [19] for comprehensive discussions and rigorous treatments of these topics. 3.4.3 MPC for Tracking: Delta Input (ÎŽu) Formulation A particularly common use case for MPC is reference tracking, where the objec- tive is to have the system state follow a reference trajectory r 0 , r 1 , . . . over time. Here, r t â R p may denote the full state reference (in which case p = n) or a partial output reference (with p < n). In this section, we introduce the ÎŽu formu- lation 18 which is particularly advantageous in trajectory tracking and regulation 18 F. Borrelli, A. Bemporad, and M. Morari. Predictive Control for Linear and Hybrid Systems. Cambridge University Press, 2017 tasks around nonzero operating points. Formally, consider the following LTI system: x t+1 = Ax t + Bu t , u t = u tâ1 + ÎŽu t , y t = Cx t , (3.58) where ÎŽu t â R m represents the change in control input from the previous time step, y t â R p denotes the system output at time t, and C â R pĂn is a linear transformation mapping the state to the output. Assuming the pair (A, B) is controllable, it is often desirable to parameterize the optimization problem in terms of the control increments rather than the absolute control inputs. Instead of optimizing the inputs u t , u t+1 , . . . , u t+Nâ1 directly, the controller optimizes their increments: ÎŽu t = u t â u tâ1 ,(3.59) and reconstructs the actual inputs recursively as: u t = u tâ1 + ÎŽu t ,(3.60) with the idea that, once the system has reached a steady state, the control inputs will remain constant (that is, ÎŽu t = 0). closed-loop control & trajectory tracking 117 As a result, the MPC problem can be readily reformulated as: J â 0 (x t ) = minimize ÎŽu 0 ,...,ÎŽu Nâ1 Nâ1 â k=0 y †k Q Ì x k + ÎŽu †k RÎŽu k , subject to x k+1 = Ax k + Bu k ,k = 0, . . . , Nâ 1, y k = Cx k ,k = 0, . . . , Nâ 1, x k âX , u k âU ,k = 0, . . . , Nâ 1, x N âX f , u k = u kâ1 + ÎŽu k ,k = 0, . . . , Nâ 1, x 0 = x(t), u â1 = u(tâ 1), (3.61) where x(t) and u(tâ 1) denote the current state and the last applied control input, respectively. Ultimately, the ÎŽu formulation does not change the fundamental structure of MPC but reparameterizes the optimization problem in a way that is better condi- tioned for tracking tasks. 3.4.4 Receding Horizon as a General Principle for Real-World Control In practice, virtually all implementations of optimal controlâwhether framed as MPC, closed-loop form, or as a two-step designâoperate according to the reced- ing horizon framework. Even when a nominal trajectory is first computed of- fline and subsequently tracked by a feedback controller, the controller is typically reinitialized and re-optimized as new state information becomes available. In other words, the nominal trajectory serves only as a temporary reference, contin- uously adjusted as the system evolves and new measurements are obtained. This viewpoint helps reconcile the apparent distinction between MPC and trajectory- tracking control: both ultimately rely on repeated, finite-horizon optimization combined with feedback, differing mainly in how frequently and to what extent the optimization is repeated. Recognizing this continuity is important, as it clar- ifies that the so-called âreceding horizonâ behavior is not unique to MPC, but rather a practical necessity in virtually all control architectures that seek robust- ness to uncertainty and disturbances. 3.5 Summary In this chapter, we introduced strategies for closed-loop control and trajectory tracking. Specifically, we discussed how the goal of closed-loop control is to compute a control policy that continuously adapts to the systemâs evolving state. While generally more computationally demanding than open-loop methods, closed- loop control provides substantial advantages in terms of robustness to distur- bances, model inaccuracies, and external perturbations. We also introduced a tractable compromise through the concept of a two-step design, in which an open-loop trajectory is first computed and then tracked us- 118 principles of robot autonomy ing a feedback controller. This formulation allowed us to naturally transition to trajectory-tracking control, bridging open-loop optimization and closed-loop stabilization. Building on these concepts, the chapter presented several key methods for closed-loop control and trajectory tracking. We began with classical feedback control, illustrated by the PID controller and its application to trajectory track- ing, particularly for differentially flat systems. We then introduced linear optimal control methods, focusing on the linear quadratic regulator, which provides an el- egant closed-form solution for linear systems via the Riccati equation and extends naturally to tracking problems. Next, we turned to nonlinear optimal control. We first contextualized tech- niques for obtaining globally optimal closed-loop solutionsânamely dynamic programming and the HJB equationâand then showed how the notion of lin- earization enables the use of LQR for local stabilization and trajectory tracking in nonlinear systems. Building on this, we discussed how algorithms such as iLQR and DDP extend these principles to simultaneously generate an open-loop trajectory and a closed-loop tracking controller in two-step design form. Finally, we presented model predictive control as a powerful framework that unifies open-loop optimization with closed-loop feedback through a receding- horizon strategy, offering a versatile method for handling constraints in real-time applications. To learn more. For a deeper exploration of the topics covered in this chapter, several key resources are available. An in-depth treatment of feedback control and PID controllers can be found in Aström and Murray [9]. For a rigorous treatment of optimal control from a dynamic programming perspective, which provides the theoretical underpinnings for the Riccati equation, Bertsekas [12] is an essential reference. Finally, for comprehensive coverage of model predictive control, from fundamental principles to advanced theory and applications, we refer the reader to the textbooks by Rawlings, Mayne, and Diehl [120] and Borrelli, Bemporad, and Morari [19]. 3.6 Exercises The starter code for the exercises provided below is available online through GitHub. To get started, download the code by running in a terminal window: git clone https://github.com/StanfordASL/pora-exercises.git We denote Problems requiring hand-written solutions and coding in Python with and, respectively. closed-loop control & trajectory tracking 119 Problem 1: Inverted Pendulum PD Control In the notebook ch03/exercises/pid _ control.ipynb, implement the PD con- troller for the inverted pendulum dynamics. Then, play around with different values of the gains k p and k d and use JAX and NumPy to compute the Jaco- bian of the closed-loop dynamics. Verify the eigenvalues of the linearized matrix are stable. Finally, run the provided code to simulate the nonlinear closed-loop dynamics. Problem 2: Cart-pole LQR Control In this problem, we consider a cart-pole formulation in which we will design a controller to balance an inverted pendulum by linearizing the dynamics around a single stationary point. Therefore, we will consider a trajectory initialized in a neighborhood about the final, upright state and the optimal control is a closed- form solution derived through Riccati recursion. This system has two degrees of freedom corresponding to the horizontal position x of the cart, and the angle Ξ of the pendulum (where Ξ = 0 occurs when the pendulum is hanging straight downwards). We can apply a force u â R to push the cart horizontally, where u > 0 corresponds to a force in the positive x-direction. With the state s : = (x, Ξ, Ì x, Ì Îž) â R 4 , we can write the continuous-time dynamics of the cart-pole system as: Ì s = f (s, u) = ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° Ì x Ì Îž m p (â Ì Îž 2 +g cos Ξ) sin Ξ+u m c +m p sin 2 Ξ â (m c +m p )g sin Ξ+m p â Ì Îž 2 sin Ξ cos Ξ+u cos Ξ â(m c +m p sin 2 Ξ) ïŁč ïŁș ïŁș ïŁș ïŁș ïŁș ïŁ» , where m p is the mass of the pendulum, m c is the mass of the cart, â is the length of the pendulum, and g is the acceleration due to gravity. We can discretize the continuous-time dynamics using Euler integration with a fixed time stepât to get the approximate discrete-time dynamics: s k+1 â s k +ât f (s k , u k ), where s k and u k are the state and control input, respectively, at time t = kât. The code for this exercise is located in ch03/exercises/cartpole _ lqr _ control.ipynb. 1. Consider the upright state Ì s : = (0, Ï, 0, 0) with Ì u : = 0, and define Ì s k : = s k â Ì s. Linearizing the approximate discrete-time dynamics s k+1 â s k +ât f (s k , u k ) about ( Ì s, Ì u) yields an approximate LTI system of the form: Ì s k+1 â A Ì s k + Bu k . 120 principles of robot autonomy Express A and B in terms of m p , m c , â, g, andât. You may use the fact that: â f âs ( Ì s, Ì u) = ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° 001 0 000 1 0 m p g m c 0 0 0 (m c +m p )g m c â 0 0 ïŁč ïŁș ïŁș ïŁș ïŁș ïŁș ïŁ» , â f âu ( Ì s, Ì u) = ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° 0 0 1 m c 1 m c â ïŁč ïŁș ïŁș ïŁș ïŁș ïŁș ïŁ» . We will design a stabilizing LQR controller for this discrete-time LTI system to solve: minimize u â â k=0 1 2 Ì s T k Q Ì s k + 1 2 u T k Ru k , subject to Ì s k+1 = A Ì s k + Bu k , âkâ N â„0 , for fixed Q, R â» 0. Recall that after N iterations of the discrete-time Riccati recursion: K k =â(R + B T P k+1 B) â1 B T P k+1 A, P k = Q + A T P k+1 (A + BK k ), the cost-to-go matrices P k N k=0 and the time-varying feedback gains K k Nâ1 k=0 describe the optimal LQR controller for a finite-horizon version of the problem above. If (A, B) is stabilizable, then these iterates asymptotically converge to some P â â» 0 and K â . In fact, J â (s 0 ) : = (s 0 â Ì s) T P â (s 0 â Ì s) is the infinite-horizon optimal cost-to-go for any initialization s 0 , and u k = K â Ì s k is the optimal feedback policy, which happens to be linear and time-invariant 19 . 19 The infinite-horizon LQR problem also converges for fixed Q âȘ° 0 and R â» 0, as long as (A, B) is stabilizable and (A, Q) is detectable. 2. Write code to approximate P â and K â for the linearized, discretized cart-pole system by initializing P â = 0 and then applying the Riccati recursion until convergence with respect to the maximum element-wise norm conditionâ„P k â P kâ1 â„ max < 10 â4 . Use m p = 2 kg, m c = 10 kg, â = 1 m, g = 9.81 m/s 2 , ât = 0.1 s, Q = I 4 , and R = I 1 . Report the value of K â with two decimal places for each entry. 3. Use the provided code to simulate the continuous-time, nonlinear cart-pole system with the linear feedback controller u = K â Ì s. Initialize the system at s = (0, 3Ï/4, 0, 0), and use a controller sampling rate of 10 Hz. Verify from the simulation output that the behavior stabilizes as expected. 4. We will now use an LQR controller to track a time-varying trajectory. Specifi- cally, we will aim to balance the pendulum upright (that is, Ì Îž(t)⥠Ï) while os- cillating the position of the cart to track a desired reference Ì x(t) = a sin(2Ït/T), where a > 0 and T > 0 are known constants. (a) Normally, as discussed in this chapter, you would have to re-linearize the system around the desired trajectory at each time step. Why is this not the case for this particular problem (that is, why can you just reuse A and B)? closed-loop control & trajectory tracking 121 (b) Repeat part (c) for this case with a = 10 and T = 10, except this time initialize the system upright at s(0) = (0, Ï, 0, 0). For each state plot, overlay the corresponding entry from the reference trajectory Ì s(t). (c) You may notice that this controller does not have good tracking perfor- mance. You could try increasing the state penalty matrix Q to, e.g., Q = 10I 4 . However, this should only improve tracking for x(t) and Ì x(t), while Ξ(t) and Ì Îž(t) still oscillate around Ï and 0, respectively. What physical char- acteristic of the desired trajectory (or lack thereof) causes this to happen? Problem 3: Cart-pole Swing Up (iLQR) In this problem, we will implement a controller to solve the cart-pole âswing upâ problem and will assume that we do not have actuation constraints. In the swing- up problem, the pendulum begins hanging downwards and is then brought to the upright position. Unlike in the cart-pole balancing problem, it is no longer sufficient to linearize around a single stationary point. Therefore, we will for- mulate the optimal control problem into a convex sub-problem and iteratively solve for optimal perturbations from a reference trajectory. Since we do not con- sider the case with limited actuation capabilities, the solution to each convex sub-problem can be solved with Riccati recursion, i.e., we will iteratively develop our solution using iterative LQR (iLQR) control. In this case, iLQR control pro- vides an elegant and easy to implement closed-loop policy. However, through the standard LQR formulation at each iteration, we generally cannot reason, at least directly, over constraints on the state or control space: in the case of limited actuation, we require the more flexible and sophisticated framework provided by Sequential Convex Programming. The code for this exercise is located in ch03/exercises/cartpole _ iterative _ lqr.ipynb. Recall that the cart-pole is a continuous-time system with dynamics of the form Ì s = f (s, u). To compute the iLQR control law, we will consider the Euler discretized dynamics: s k+1 â f (s k , u k ) : = s k +ât f (s k , u k ),(3.62) with time stepât > 0. The provided code will then simulate this control law on the original continuous-time system. 1. For a given operating point ( Ì s k , Ì u k ), suppose we define the Jacobians: A k : = â f âs ( Ì s k , Ì u k ),B k : = â f âu ( Ì s k , Ì u k ).(3.63) Use JAX in the function linearize to write a single line of code that computes A k and B k , given f , Ì s k and Ì u k . For our iLQR controller, we will use the quadratic cost function: J(s, u) : = 1 2 (s N â s goal ) T Q N (s N â s goal ) + 1 2 Nâ1 â k=0 (s k â s goal ) T Q(s k â s goal ) + u T k Ru k , (3.64) 122 principles of robot autonomy where s goal is the goal state (i.e., the upright position). The entries of Q N â» 0 are chosen to be large so that the terminal cost acts as a soft terminal âconstraintâ. 2. Rewrite the cost function in terms of the deviations Ì s N : = s N â Ì s N , Ì s k : = s k â Ì s k , and Ì u k : = u k â Ì u k . This will result in a quadratic cost function with linear terms of the form q T N Ì s N , q T k Ì s k , and r T k Ì u k . Identify the vectors q N , q k , and r k . 3. Complete the iLQR controller code in the function discreteIterativeLQR.solve. Specifically, your code must update the con- troller gain and offset termsY k Nâ1 k=0 andy k Nâ1 k=0 , respectively 20 , the nominal 20 In Section 3.3.3, we labeled these terms as K t and k t , but here we try to avoid confusion with the discrete index k. trajectory ( Ì s, Ì u), and the deviations ( Ì s, Ì u). 4. Implement the function compute _ control for the discreteIterativeLQR class to apply either the open-loop iLQR control input or the closed-loop iLQR policy, depending on the value of the Boolean flag closed _ loop. Run your code for both cases to simulate the system and generate plots of the state and control input over time. You should notice that the iLQR control sequence does not accomplish the task if applied open-loop. Problem 4: Linear MPC In Section 3.4, we introduced some of the fundamental aspects of model predic- tive control. For this exercise, you will implement a simple MPC scheme for a linear system to drive the system to the origin while minimizing the cost function: J(x 0 ) = â â t=0 x †t Qx t + u †t Ru t , and satisfying the inequality constraints on the state and control, x t †x u , x t â„ x l , and â„u t â„ â †r u . To solve this problem with a finite-horizon MPC scheme, we will use the cost function from Equation (3.57) which adds a terminal cost and we will also add a terminal constraint â„x N â„ â †r N to the problem. In the notebook ch03/exercises/linear _ mpc.ipynb: 1. Implement the function solve _ mpc to solve a finite horizon MPC problem with the quadratic stage and terminal costs, inequality constraints on the state and control, and the terminal constraint discussed above. 2. Run the provided code to simulate the closed-loop system with the terminal cost Q f = Q and with no terminal constraint (that is, set r N =â). Analyze the resulting plots, explain why there is a difference between the trajectory computed in each MPC iterationâs optimization problem and the final closed- loop trajectory. 3. Next, run the provided code that specifies a different terminal cost matrix Q f by the solution to the discrete-time algebraic Riccati equation (DARE) from Equation (3.22) in the section on the linear quadratic regulator. What do you closed-loop control & trajectory tracking 123 notice about the difference between the MPC trajectory computed at each time step and the closed-loop system behavior? Why is this different than when using Q f = Q? Notice that the total cost of the closed-loop trajectory is lower when using the DARE terminal cost matrix, why is that? 4. As we discussed in Section 3.4, persistent feasibility is a challenge with MPC, where we want to guarantee that if we find a feasible solution at one time step then we will be able to find a feasible solution at future time steps. One simple solution to this problem is to apply a terminal constraint x N = 0, which forces the optimizer to find a trajectory to the origin by the end of the finite horizon, thereby eliminating the issue with being short-sighted. However, this will cause the controller to be sub-optimal and can require a longer horizon to find a feasible solution. Run the provided code and play around with the controllerâs horizon, how does the closed-loop trajectoryâs cost change as the horizon changes? Problem 5: Extended Unicycle Trajectory Tracking Control In Example 3.1.2, we introduced a closed-loop trajectory tracking problem for a dynamically extended unicycle model where we apply a PD controller. In this example, we showed that since the system is differentially flat we could write its dynamics as the second order linear system in Equation (3.6). From these dynamics, we can develop a PD controller for the virtual inputs w 1 and w 2 given by Equation (3.7), and then can map these virtual controls back into the actual controls (a, Ï) algebraically using: " a Ï # = J â1 (Ξ, v) " w 1 w 2 # . In this exercise, you will compute a desired reference trajectory for the ex- tended unicycle and then will design a PD controller to track the reference in the presence of external disturbances. The code implementation aspects of the exercise can be completed in the notebook ch03/exercises/unicycle _ trajectory _ tracking.ipynb. 1. First, you will design the reference trajectory for the unicycle to follow. While there are several ways to accomplish this, for this exercise you will use a poly- nomial basis expansion of the form: x d (t) = n â i=1 x i Ï i (t),y d (t) = n â i=1 y i Ï i (t), where Ï i for i = 1, . . . , n are the basis functions, and x i and y i are coefficients that can be selected. Use the basis functions Ï 1 (t) = 1, Ï 2 (t) = t, Ï 3 (t) = t 2 , Ï 4 (t) = t 3 . 124 principles of robot autonomy (a) Write a set of linear equations in the coefficients x i and y i for i = 1, . . . , 4 to express the following initial and final conditions: x(0) = x 0 ,y(0) = y 0 ,v(0) = v 0 ,Ξ(0) = Ξ 0 , x(t f ) = x f ,y(t f ) = y f ,v(t f ) = v f ,Ξ(t f ) = Ξ f . (b) Implement the function compute _ traj _ coeffs to compute the coefficients x i and y i . (c) Then, implement the function compute _ traj to use the coefficients and the basis functions to compute the full reference trajectory x d (t) and y d (t). Additionally, compute _ traj should also compute v d (t) 21 , Ξ d (t), Ì x d (t), Ì y d (t), 21 Hint: from the dynamics model we can see v = p Ì x 2 + Ì y 2 . Ì x d (t), and Ì y d (t). (d) Now, implement the function compute _ controls to compute a(t) and Ï(t) for a given state trajectory. Run the provided code to compute a trajectory and control sequence for the initial and final conditions: x(0) = 0,y(0) = 0,v(0) = 1 2 ,Ξ(0) =â Ï 2 , x(t f ) = 5,y(t f ) = 5,v(t f ) = 1 2 ,Ξ(t f ) =â Ï 2 , with t f = 25. Why do we have to choose v(t f ) > 0? What would happen if we let v(t) = 0 at some point along the trajectory? (e) Run the provided code to simulate the unicycle system with the controls a(t) and Ï(t) computed previously in open-loop while subject to disturbances. Notice how the system does not reach the target state in the presence of noise. 2. Now that we can compute a reference trajectory for the unicycle to follow, we want to compute a closed-loop controller to track the trajectory robustly in the presence of disturbances. Specifically, you will use the PD controller from Equation (3.7) to compute the virtual controls w 1 and w 2 . (a) Write down a system of equations for computing the control inputs a and Ï in terms of the virtual controls w 1 = Ì x and w 2 = Ì y and the unicycle state x = [x, y, v, Ξ] †. Use this to implement the function TrajectoryTracker.compute _ control. (b) Run the provided code to run the closed-loop tracking controller. How does the performance compare to the open-loop? Experiment with different starting and ending conditions, and different amounts of noise. closed-loop control & trajectory tracking 125 References [9]K. J. Aström and R. M. Murray. Feedback Systems. Princeton University Press, 2009. [12]D. Bertsekas. Dynamic Programming and Optimal Control. Athena Scientific, 2000. [19]F. Borrelli, A. Bemporad, and M. Morari. Predictive Control for Linear and Hybrid Systems. Cambridge University Press, 2017. [80]J. Levine. Analysis and Control of Nonlinear Systems: A Flatness-based Ap- proach. Springer, 2009. [101]R. M. Murray. Optimization-Based Control. California Institute of Technol- ogy, 2009. [120]J. Rawlings, D. Q. Mayne, and M. Diehl. Model Predictive Control: Theory, Computation, and Design. Nob Hill Publishing, 2017. [146]Y. Tassa. Theory and Implementation of Biomimetic Motor Controllers. PhD Thesis. The Hebrew University of Jerusalem, 2011. 4 Motion Planning As introduced in Chapters 2 and 3, autonomous systems transform high-level goals into concrete physical actions through a hierarchy of processes, ranging from strategic decision-making to real-time control. While open-loop trajectory optimization offers a principled framework for generating dynamically feasible state and control sequences, it often struggles to handle complex, state-dependent constraints, such as collision avoidance. Moreover, trajectory optimization meth- ods typically yield only locally optimal solutions, whose quality can strongly de- pend on initialization and may fail to capture the global structure of the problem. To address these limitations, this chapter focuses on motion planning, which takes a more global perspective by explicitly reasoning about the full set of envi- ronmental constraints, obstacles, and trajectory feasibility. To maintain compu- tational tractability, motion planning often relies on simplified dynamical mod- els, producing coarse or geometric paths that ensure collision-free navigation. In practice, motion planning and trajectory optimization often complement each other, with motion planning establishing global feasibility by finding an initial collision-free path, and trajectory optimization refining this path into a smooth, dynamically consistent, and optimized trajectory. The study of the motion planning problem, formally defined in the 1970s, has a rich history. Early work in the 1980s focused on exact, combinatorial algorithms designed to capture the geometry of configuration spaces with mathematical rigor. While theoretically elegant, these methods suffered from severe computa- tional bottlenecks in high-dimensional problems. The 1990s brought a paradigm shift with the introduction of sampling-based approaches, which offered scal- able solutions for complex motion planning problems. The 2000s marked the widespread deployment of planning algorithms on real-time systems, from au- tonomous vehicles to robotic manipulators. Today, research in motion planning is vibrant and expanding, focusing on integrating differential and logical con- straints, planning under uncertainty, leveraging parallel computation, and incor- porating learning-based methods. The relevance of motion planning is reflected in its breadth of applications. To name a few, in autonomous driving, planning algorithms compute safe and efficient maneuvers for vehicles navigating dense traffic. In humanoid robotics, This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 128 principles of robot autonomy planners coordinate whole-body motions for tasks such as walking, climbing, or manipulation in cluttered environments. In surgical robotics, motion planning is used to design precise, collision-free tool trajectories inside the human body. Even outside classical robotics, motion planning techniques have found applications in fields such as protein folding, where high-dimensional configuration spaces must be navigated to determine protein folding pathways. Over the past decades, a diverse range of algorithmic approaches has been developed to address motion planning problems. These approaches are com- monly grouped into four main categories: grid-based planning, combinatorial planning, sampling-based planning, and potential field methods. At a high level, grid-based planners discretize the robotâs environment into a grid and use graph search algorithms to compute a feasible path through the grid cells. Combinatorial planners construct explicit representations of the configuration space that capture the connectivity of the free space required for planning. Sampling-based planners leverage random sampling and collision detection to incrementally explore the configuration space, offering scalability to high-dimensional problems where ex- plicit representations become intractable. Finally, potential field methods construct artificial potential functions that attract the robot toward its goal while pushing it away from obstacles. In this chapter, we begin by formally defining the motion planning prob- lem in Section 4.1, introducing the core concepts and notation. We then ex- plore the four major algorithmic paradigms for motion planning discussed above, namely grid-based planning in Section 4.2, combinatorial planning in Section 4.3, sampling-based planning in Section 4.4, and potential field methods in Section 4.5. 4.1 Problem Formulation We begin by formally defining the motion planning problem in its most basic for- mulation. LetW â R 2 denote the robotâs workspace, i.e., the physical environment in which the robot operates. Within this workspace lies a known obstacle region O â W , characterized by a polygonal (piecewise-linear) boundary. The robot itself is modeled as a rigid polygon that must navigate through the workspace without intersecting any obstacles 1,2 . 1 For a 3D workspace, the only differ- ences are that W â R 3 , and the robot and the obstacle region are represented as polyhedra. 2 The basic formulation presented here extends well beyond rigid polygons and polyhedra to include robots with com- plex geometries. For an in-depth treat- ment of these extensions, we refer the reader to LaValle [77]. With this setup, the fundamental motion planning problem can be informally stated as follows: given an initial placement of the robot, compute how to gradually move it into a desired goal placement without colliding with any obstacles. As an illus- tration, consider the workspace in Example 4.1.1, where the task is to move an L-shaped robot from its initial position to a target position while avoiding polyg- onal obstacles. In this case, the output of a motion planning algorithm will be a path through the set of all intermediate transformations of the robot, from start to goal. Although the motion planning problem is naturally described in the robotâs workspace, it really lives in another space: the set of all rigid-body transforma- tions that describe the robotâs possible placements. As discussed in Chapter 1, motion planning 129 Figure 4.1:Motion planning in a two-dimensional workspace with obstacles.The task is to move the L-shaped robot from start (bottom-left) to goal (top- right) without collisions. Figure 4.2: Configuration space defined on R 2 ĂS 1 .Rotating clockwise from Ξ 0 to Ξ g leads to collision, while rotating counter- clockwise yields a feasible path. this set is referred to as the configuration space orC-space. In this formulation, ob- stacles in the workspace induce forbidden regions inC, and the motion planning problem reduces to finding a continuous path inC that avoids collisions. Let us briefly review the notion of configuration space through an example. Example 4.1.1 (L-shaped Robot). Consider the L-shaped robot in Example 4.1.1. The task is to move it from an initial placement to a goal placement in a two- dimensional world with polygonal obstacles. Suppose the robot is described by a state: x = h xy Ξ Ì x Ì y Ì Îž i †, where (x, y) denote position, Ξ denotes orientation, and ( Ì x, Ì y, Ì Îž) denote veloci- ties. For the simplest version of motion planning, we restrict the attention to the robotâs configuration: q = h xy Ξ i †, which fully describes its degrees of freedom. In other words, every combina- tion of (x, y, Ξ) corresponds to a unique placement of the robot in the workspace, and is called a configuration. This abstraction simplifies the problem: instead of computing a trajectory for the full state, we seek a sequence of collision- free configurations, as shown in the right-side graphic of Example 4.1.1. The resulting geometric path in configuration space can then be provided to trajec- tory optimization methods for refinementâallowing them to incorporate system dynamicsâand subsequently to closed-loop control methods (e.g., tracking con- trollers introduced in Chapter 3) for execution. In this example, the configuration space isC = R 2 ĂS 1 â R 3 , where R 2 represents the robotâs position in the plane 130 principles of robot autonomy Figure 4.3: Free (C free ) and for- bidden (C obs ) spaces of the con- figuration space for a circular robot in the presence of a polyg- onal obstacle.The forbidden region accounts for the robotâs physical dimensions. andS 1 is the one-dimensional unit circle manifold representing the robotâs orien- tation. The presence of the manifoldS 1 reflects the periodicity of the orientation variable Ξ, where Ξ and Ξ± 2Ïk are equivalent for all integers k. This periodic- ity has important practical implications for planning, as the robot can reach the same orientation either by rotating clockwise or counter-clockwise 3 . In the sce- 3 For instance, a heading change of Ï/2 radians can be achieved by turning left by Ï/2 or turning right by 3Ï/2. nario depicted in Example 4.1.1, suppose the robot has an initial heading Ξ 0 and a goal heading Ξ g . If one were to consider only clockwise rotations, there would be no feasible path to the goal without colliding with the obstacle. However, once periodicity is properly accounted for by modeling orientation asS 1 , the robot can simply rotate counter-clockwise to reach the goal without collision. Therefore, in this example, the motion planning problem reduces to finding a continuous path inC = R 2 ĂS 1 that avoids the forbidden regions induced by the obstacles in the workspace. Crucially, the concept of configuration space generalizes to robots with more complex geometries and higher degrees of freedom (e.g., robotic arms with mul- tiple joints). Free space in configuration space. A fundamental concept in motion planning is the free space, denoted byC free . Intuitively,C free is the set of all robot configurations in which the robot does not collide with any obstacles in the workspace. Formally, let R(q) â W denote the set of points in the workspace occupied by the robot when placed at configuration q. The free space is defined as: C free =qâC| R(q)â©O =â .(4.1) Similarly, the obstacle region in configuration space, denoted byC obs , is defined as the complement of the free space: C obs =qâC | R(q)â©O Ìž=â .(4.2) To illustrate these definitions, consider the simple case of a circular robot navigat- ing among polygonal obstacles, as shown in Section 4.1. Although the robot itself is a disk of nonzero radius, collision checking in configuration space reduces to verifying that a point-like representation of the robot (its configuration) does not intersect the obstacle regions in configuration space. In this case, this equivalence is achieved by inflating the workspace obstacles by the robotâs radius. The red motion planning 131 Figure 4.4: Example of a path planning problem in configura- tion space. The free space C free excludes the forbidden config- urations C obs induced by the polygonal obstacles.Motion planning reduces to finding a continuous path from the start to the goal within C free . Note that for a simple point robot in 2D, the C-space and the workspace would coincide. region in the figure represents the obstacle in the physical workspace,O, whereas the grey region corresponds to the inflated obstacle in configuration space,C obs . Once C free (and, correspondingly, C obs ) has been computed, the robotâs physi- cal dimensions no longer need to be explicitly considered, and the robot can be treated as a point moving through the configuration space, as illustrated in the previous example. With this abstraction, the geometric complexity of the robot and the environments are absorbed into the structure of the configuration space itself. Within this abstracted setting, the motion planning problem reduces to finding a continuous path Ï : [0, 1] â C free such that Ï(0) = q S and Ï(1) = q G , where q S and q G denote the start and goal configurations, respectively. An example is shown in Section 4.1, where the robot must navigate around forbidden regions in C-space to reach the goal. By working in configuration space, the problem of collision-free navigation becomes purely geometric, focused on finding a path that avoids the forbidden regions 4 . 4 As we will see in Section 4.4.5, this purely geometric interpretation is true for static environments and without kin- odynamic constraints. In the remainder of this chapter, we explore various algorithmic strategies that aim to solve either discretized (Section 4.2) or continuous (Sections 4.3-4.5) ver- sions of the motion planning problem. 4.2 Grid-based Motion Planning A natural way to simplify the motion planning problem is to discretize the robotâs continuous configuration space into a grid. Instead of reasoning about infinitely many possible configurations, we approximate the environment by dividing it into a finite set of cells. Each cell is classified as either free (i.e., collision-free, meaning the robot can occupy it) or forbidden (i.e., meaning it would lead to a 132 principles of robot autonomy collision with an obstacle). The robot is allowed to move between adjacent free cells, and the planning task reduces to finding a sequence of connected free cells that leads from the start to the goal. This discretization naturally converts motion planning into a graph search prob- lem. Specifically, each free cell in the grid is represented as a vertex in a graph, and an edge is added between two vertices whenever the corresponding cells are adjacent and both free. Formally, we construct a graph G = (V ,E), where each vertex v â V corresponds to a free cell, and each edge (v, u) â E corresponds to a valid robot move between adjacent cells. Planning then amounts to finding a path in the graph from the start vertex to the goal vertex. This perspective is powerful because it allows motion planning to draw on decades of results from graph theory and computer science. Algorithms origi- nally developed for solving shortest-path problemsâsuch as breadth-first search or Dijkstraâs algorithmâcan be directly applied to motion planning. In other words, the seemingly geometric problem of ânavigating through a space with obstaclesâ is reduced to the purely combinatorial problem of âfinding a path through a graphâ. This approach, however, relies on an explicit characterization of C free , which is necessary to construct the grid and classify each cell as either free or occupied. In practice, obtaining such an exact characterization is often infeasibleâparticularly in high-dimensional configuration spacesâthereby moti- vating alternative strategies, such as sampling-based planning, discussed later in this chapter. Connectivity and neighborhood structure. The way adjacency is defined depends on the robot model and the chosen grid structure: âą In a 4-connected grid, each cell has up to four neighbors (up, down, left, right). âą In an 8-connected grid, diagonal moves are also allowed, giving each cell up to eight neighbors. âą More generally, one can consider k-connected neighborhoods (e.g., 16-connected) or hexagonal tilings, trading off simplicity, path quality, and computational cost. The choice of connectivity has practical consequences. A coarse neighborhood may restrict the robotâs ability to approximate smooth trajectories, while a richer neighborhood increases branching factors and search complexity. Regardless of the specific choice, the key point is that the grid structure induces a well-defined graph on which search algorithms can operate. 4.2.1 Label Correcting Algorithms Once the motion planning problem has been cast as a graph search, the next step is to design an algorithm that systematically explores the graph to find a path from start to goal. A broad and powerful family of methods for this task is known as label-correcting algorithms. motion planning 133 The term âlabelâ refers to a numerical value associated with each vertex in the graph, which represents the cost of the best-known path from the start vertex to that vertex. At a high level, label correcting algorithms proceed by iteratively refining these cost labels. Initially, only the start vertex has a cost label of zero, while all others are initialized to infinity (or an undefined state). The algorithm proceeds by exploring the graph and updating, or correcting, these labels when- ever a better (lower-cost) path to a vertex is discovered. Formally, let q S denote the start vertex and q G the goal vertex 5 . For each vertex 5 We use the notation q for graph ver- tices to emphasize the connection be- tween graph search and path planning in configuration space, where q typically denotes a configuration. q, let C(q) represent the label, i.e., the cost of the best path found so far from q S to q. This value is also known as the cost of arrival. Label-correcting algorithms operate by maintaining a frontier (also referred to as the alive set or a priority queue) of vertices whose neighbors may still admit cost improvements. At each iteration, a vertex q is extracted from the frontier and expanded. Expansion refers to the process of examining all outgoing edges from q to its neighboring vertices q âČ . For every neighbor 6 q âČ of q, the algorithm attempts 6 In graph theory, two vertices are de- noted as neighbors if they are connected by an edge. to improve (or relax) the current cost estimate associated with q âČ . Specifically, given an edge (q, q âČ ) with cost c(q, q âČ ), the relaxation step verifies whether the path through q offers a lower cost of arrival to q âČ than any previously known pathânamely, whether: C(q) + c(q, q âČ ) < C(q âČ ). If this condition holds, the label of q âČ (i.e., C(q âČ )) is updated and q âČ is reinserted into the frontier for further exploration. This process continues until no label can be further improved. General structure of label-correcting algorithms. All algorithms in this family share the following structure: âą Initialization: Set C(q S ) = 0 and C(q) =â for all other vertices q. Initialize the frontier with the start vertex Q =q S . âą Main Loop: Until the frontier Q is not empty, repeat: 1. Extract a vertex q from the frontier Q according to a specific selection rule 7 . 7 The choice of selection rule is the main distinguishing factor between different label-correcting algorithms. 2. For each neighbor q âČ of q, perform the relaxation step, i.e.: (a) Check if C(q) + c(q, q âČ ) < C(q âČ ). (b) If so, update C(q âČ ) â C(q) + c(q, q âČ ) and add q âČ to the frontier Q if it is not already present. âą Termination: The algorithm terminates when the frontier is empty. At this point, C(q) contains the cost of the shortest path from q S to each reachable vertex q. The pseudocode for the general structure of a label-correcting algorithm is pro- vided in Algorithm 4.1. 134 principles of robot autonomy Algorithm 4.1: Structure of a Label-Correcting Algorithm Data: q S , q G , directed graphG = (V ,E), edge costs c(·,·), selection rule Select Result: Shortest path from q S to q G (if reachable) for qâV\q S do C(q)ââ C(q S )â 0 Qâq S / * Initialize priority queue * / while Q is not empty do qâ Select(Q); Q.remove(q) for q âČ â u| (q, u)âE do Ì C â C(q) + c(q, q âČ ) if Ì C < C(q âČ ) then C(q âČ )â Ì C;parent(q âČ )â q if q âČ Ìž= q G and q âČ /â Q then Q.add(q âČ ) if C(q G ) =â then return N.A. / * Goal unreachable * / else / * Reconstruct path by backtracking parents * / pathâ [ ]; qâ q G while qÌž= q S do path.prepend(q); qâ parent(q) return path The role of the selection rule. The selection rule for choosing the next vertex to expand is one of the key factors that differentiates various label-correcting al- gorithms, ultimately defining their efficiency and performance. Several classical search methods can be seen as special cases of the label-correcting framework, for example: âą Depth-First Search (DFS): in DFS (Figure 4.5), the frontier is managed as a stack (last-in, first-out). This means that the algorithm always continues along the most recently discovered path before backtracking. DFS is appealing for its simplicity and low memory footprint, since the number of active nodes is pro- portional to the depth of the search. However, DFS is exposed to the risk of getting trapped in deep but unproductive branches of the search tree, leading to poor performance in finding the shortest path. 0 1 2 5 3 4 6 Figure 4.5: Depth-first search âą Breadth-First Search (BFS): in BFS (Figure 4.6), the frontier is a queue (first-in, first-out). The algorithm expands all nodes at a given âdepthâ before moving on to the next, effectively exploring the graph in concentric layers around the start node. Compared to DFS, BFS requires significantly more memory, as it motion planning 135 must store all frontier nodes at a given depth. 0 1 2 3 4 5 Figure 4.6: Breadth-first search âą Dijkstraâs Algorithm (also known as Best-First Search): in Dijkstraâs algorithm (Al- gorithm 10), the frontier is a priority queue ordered by the tentative label C(q), i.e., the best cost of arrival discovered so far. At each iteration, the vertex with the smallest label is selected for expansion. Formally, we express this greedy selection of the next vertex as: Select(Q) = arg min qâQ C(q).(4.3) A key property of Dijkstraâs algorithm is that once a vertex is extracted from the priority queue, its label is guaranteed to be final and optimal. In other words, each node will be expanded at most once, leading to a more efficient exploration of the graph. Because of this, Dijkstraâs algorithm provides the foundation for many widely used planning methods such as A â and D â . Dijkstraâs Algorithm def dijkstra(start _ node, edges): visited = set() Q = [(0, start _ node)] while Q: current _ cost, current _ node = heapq.heappop(Q) if current _ node in visited: continue visited.add(current _ node) for neighbor, edge _ cost in edges[current _ node]: if neighbor in visited: continue new _ cost = current _ cost + edge _ cost if new _ cost < neighbor.cost: neighbor.cost = new _ cost neighbor.parent = current _ node.idx heapq.heappush(Q, (new _ cost, neighbor)) Algorithm 10: Python implementation of Dijkstraâs algorithm. See this code used in the context of motion planning in the repository github.com/StanfordASL/p ora-exercises in the notebook ch04/prm _ star.ipynb. To make the differences between these search strategies more concrete, consider the simple graph in Section 4.2.1. In this example, the start vertex is A and the goal vertex is E. The numbers in parentheses indicate the order of expansion for each of the three search strategies, and the numbers next to each edge indicate the cost of traversing that edge. By observing the order of expansion, we can see how each strategy explores the graph differently, with DFS going deep into one 136 principles of robot autonomy branch, BFS exploring all nodes at the current depth before moving deeper, and Dijkstraâs algorithm expanding nodes based on the lowest cumulative cost. A(1) B C(2) D(3) E(4) 1 5 11 1 (a) DFS (stack) A(1) B(2) C(3) D(4) E(5) 1 5 11 1 (b) BFS (queue) A(1) B(2) C D(3) E(4) 1 5 11 1 (c) Dijkstra (priority by C) Figure 4.7: Effect of the selec- tion rule on exploration order. Numbers inside nodes indicate the order of expansion (removal from the frontier).DFS fol- lows the most recently discov- ered branch and may yield a suboptimal-cost route; BFS ex- plores all nodes at the current depth before proceeding deeper; Dijkstra expands in order of lowest cost from the start node. Beyond Dijkstra: toward more informed search for motion planning. Dijkstraâs algo- rithm is one of the most widely used methods for graph search, owing to its simplicity and the guarantee that it always finds an optimal path, if one exists 8 . 8 D. Bertsekas. Dynamic Programming and Optimal Control. Athena Scientific, 2000 In particular, Dijkstra is a correct algorithm, in that it always finds a least-cost path from the start node to the goal node, provided such a path exists. Theorem 4.2.1 (Correctness of a graph search algorithm; Bertsekas [12]). If a fea- sible path exists from q S to q G , then the algorithm terminates in finite time with C(q G ) equal to the optimal cost of traversal, C â (q G ). While this property makes Dijkstra appealing, its exploration strategy may result in significant wasted effort. By always expanding the frontier node with the smallest accumulated cost of arrival, Dijkstra effectively explores the search space in âcost contoursâ radiating outward from the start. This strategy guarantees optimality for the path that is returned, but may expand many vertices that are irrelevant for reaching the goal, a drawback that becomes especially pronounced in the large and structured graphs typical of motion planning (see Figure 4.8). These limitations motivated the development of more informed search strate- gies that incorporate additional guidance toward the goal. The most influential among these are the A* algorithm, which augments Dijkstra with heuristic es- timates of the remaining cost to the goal, and its dynamic extension, the D* algorithm, which adapts the search as new information about the environment becomes available. q S q G Figure 4.8:Dijkstraâs expan- sion in grid-based motion plan- ning.The algorithm explores from q S by selecting from the frontier (crosshatch-filled nodes) the node with the lowest cost of arrival (the solid-filled nodes have been previously visited). By doing so, the exploration process may include regions that do not help reach q G . The A* algorithm. The A â algorithm improves upon Dijkstraâs selection rule by augmenting the cost of arrival with a heuristic estimate of the cost-to-go, i.e., the cost from the current node to the goal. Formally, instead of expanding the vertex that minimizes C(q), A â expands the vertex that minimizes: f (q) = C(q) + h(q),(4.4) motion planning 137 where h(q) is a heuristic function estimating the optimal remaining cost from q to the goal. Consequently, the relaxation step for each neighbor q âČ of q is also strengthened to update f (q âČ ) rather than just the cost of arrival C(q âČ ): f (q âČ )â min f (q âČ ), C(q) + c(q, q âČ ) + h(q âČ ) .(4.5) As long as h(·) is admissible (i.e., it never overestimates the true cost-to-go), A â is guaranteed to return an optimal path. Intuitively, underestimating the cost-to-go ensures that nodes are not prematurely discarded, which could otherwise lead to suboptimal solutions. In practice, heuristics such as the Euclidean or Manhattan distance to the goal often reduce the number of vertices explored, since the search is biased toward the target rather than expanding uniformly in all directions. This makes A â one of the most widely used graph search algorithms for the purposes of motion planning. The D* algorithm. In many robotic applications, the environment is only par- tially known in advance, and new information (such as previously unseen ob- stacles) may be discovered during execution. Recomputing an A â search from scratch each time the map changes can be computationally challenging. The D â algorithm (Dynamic A*) addresses this challenge by incrementally repairing the solution when changes are detected. Rather than discarding the existing search tree, D â efficiently updates cost labels and frontier priorities, reusing past com- putations whenever possible. This makes it particularly suitable for autonomous navigation in dynamic or uncertain environments, where the robot must adapt its plan online as new information is gathered. For an in-depth treatment of D â , we refer the reader to Stentz [141]. Pros and cons of grid-based planning. Grid-based planning methods offer several appealing advantages that explain their long-standing popularity in robotics. Perhaps the most important benefit is their simplicity: the underlying idea of discretizing the configuration space into cells and treating planning as a graph search problem is straightforward to implement and reason about. Once the grid is constructed, classical search algorithms such as DFS, BFS, or Dijkstra can be directly applied. This also makes grid-based methods relatively fast in certain settings, particularly when the resolution of the grid is well-matched to the scale of the environment and the complexity of the obstacles. Despite these advantages, grid-based methods come with important limita- tions. A first challenge is that they are inherently resolution dependent. If the grid resolution is too coarse, narrow passages or fine obstacle boundaries may be missed, and the planner may fail to find a feasible solution even if one exists in the continuous space. On the other hand, using a very fine grid increases the computational burden significantly, as the number of grid cells grows rapidly with finer resolution. Thus, achieving the right tradeoff between resolution and tractability is nontrivial. 138 principles of robot autonomy A second drawback is that grid-based planning scales poorly with robot com- plexity. While it is effective for simple 2D robots moving in two-dimensional workspaces (i.e., where configuration space and workspace coincide), the size of the grid grows exponentially with the number of degrees of freedom (DOFs) of the robot. Moreover, grid-based planning requires an exact characterization of the free space C free , which is often difficult to compute in high-dimensional con- figuration spaces. These limitations motivate the development of sampling-based methods, which are discussed in Section 4.4. As a result, grid-based methods are primarily used for robots with few DOFs or for simplified planning problems where the dimensionality of the configuration space is deliberately reduced. In summary, grid-based planning remains a widely used and important ap- proach, particularly for low-dimensional problems. However, its reliance on dis- cretization and its exponential scaling with dimension limit its applicability to more complex robotic systems. 4.3 Combinatorial Motion Planning In this section, we return to the continuous formulation of the motion planning problem and explore combinatorial approaches. The key idea is to construct an exact representation of the connectivity of the free space C free without resorting to approximations. Instead of discretizing the space arbitrarily, combinatorial methods compute a roadmap that captures the essential topological structure of C free to enable planning in continuous spaces. Due to this property, combinatorial motion planning algorithms are referred to as exact, as they find paths through the continuous configuration space without resorting to approximations. Combinatorial planners are also complete, meaning they are guaranteed to find the optimal path if one exists, or correctly report fail- ure otherwise. This is in contrast to grid-based planners, which are only resolution complete, guaranteeing a solution only if one exists at the chosen discretization resolution. However, like grid-based methods, combinatorial planning becomes computa- tionally challenging in high-dimensional configuration spaces, since computing the exact geometry of C free , and its decomposition into a roadmap, is often pro- hibitively expensive. As a result, such approaches are best suited for robots with a small number of degrees of freedom or for low-dimensional planning problems. The roadmap. A roadmap is a graph G = (V ,E) embedded in the configura- tion space, where each vertex q â V corresponds to a configuration in C free , and each edge (q, q âČ ) â E represents a continuous, collision-free path between the corresponding configurations. Let S â C free denote the set of all configurations represented by the vertices inG. ForG to be a valid roadmap, it must satisfy two key conditions that ensure it accurately represents the structure ofC free : 1. Accessibility: from any configuration q â C free , it must be simple to compute a continuous, collision-free path to any configuration s â S . Typically, s is motion planning 139 chosen as the nearest vertex to q (assumingC is a metric space). This condition ensures that every configuration in the free space can be connected to the roadmap without leavingC free . 2. Connectivity-preserving: using the accessibility condition, it must be possible to connect any two configurations q S , q G â C free to some s 1 , s 2 in the roadmap, respectively. The connectivity-preserving property requires that if there exists a continuous, collision-free path between q S and q G in C free , then there must also exist a corresponding path between s 1 and s 2 in the roadmap G. In other words, no feasible path in C free is lost because the roadmap fails to capture the underlying connectivity of the free space. This property is essential for the completeness of combinatorial planning algorithms. Once these conditions are satisfied, the roadmap provides an exact representation of the planning problem, where vertices correspond to representative configura- tions in accessible regions of C free , and edges encode the connectivity between them. Motion planning then reduces to a graph search problem, where the start and goal configurations are connected to the roadmap and a path is searched for in the resulting graph. While this structure may appear similar to the grid-based approach discussed earlier, the key distinction lies in how the space is represented. A roadmap is constructed to capture the true geometry and connectivity of C free , rather than imposing a fixed discretization ofC onto a uniform grid. In other words, vertices in a roadmap can correspond to any configuration within the free space, allowing feasible paths to always be preserved in the graph representation, whereas grid- based methods may fail to represent certain valid paths due to the coarseness of the underlying discretization. This two-step procedureâroadmap construc- tion followed by graph searchâprovides a powerful and general framework for motion planning 9 . 9 For an in-depth treatment of the con- cepts of roadmaps, and a formal defi- nition of the requirements for complete- ness and optimality, see LaValle [77]. 4.3.1 Cell Decomposition As discussed above, combinatorial methods must construct a finite data structure that exactly encodes the planning problem. One way to achieve this is through cell decomposition methods, which partition the free space C free into a finite collection of regions, called cells, that can be used to construct a roadmap. A useful way to think about cell decompositions is through three key proper- ties that make them suitable for motion planning: 1. Trivial connectivity within cells: computing a path from one configuration to another inside a cell must be easy. For instance, if every cell is convex, then any two points in the cell can be connected by a straight-line segment that remains inC free . 2. Adjacency extraction: it must be straightforward to determine which cells are adjacent to one another, so that a roadmap can be built by connecting neigh- boring cells. 140 principles of robot autonomy 3. Efficient query location: for a given initial and goal configuration (q S , q G ), it should be efficient to determine which cells contain them. When these conditions are satisfied, the motion planning problem reduces to a graph search problem, where vertices correspond to representative configurations in each cell, and edges connect neighboring cells. Vertical cell decomposition. A widely used technique for 2D environments is the vertical decomposition, also called trapezoidal decomposition. Suppose the obstacles are polygonal, and let P denote the set of vertices defining C obs . At each vertex p â P, rays are extended vertically upward and downward through C free until they intersect either another obstacle or the workspace boundary. Depending on the local geometry, four distinct cases arise, corresponding to whether a vertical extension is possible upward, downward, in both directions, or in neither, as illustrated in Section 4.3.1. (1) Up and down(2) Up only(3) Down only(4) None Figure 4.9: Four general cases of vertical ray extension from a ver- tex of a polygonal obstacle. The result of this process, illustrated in Example 4.3.1, is a decomposition of C free into a collection of 2-cells (open trapezoids and degenerate triangles) and 1-cells (open vertical line segments forming the boundaries between trapezoids). Once the decomposition is available, a roadmapG = (V ,E) can be constructed. For each 2-cell C i , a representative sample point q i â C i is chosenâcommonly the centroid, though the exact choice is not critical. Each 1-cell is also assigned a sample point. These points are visualized as black dots in Example 4.3.1. The roadmap graph is then defined as follows: every cell corresponds to a vertex, and for each 2-cell, edges are added to connect its sample point with the sample points of adjacent 1-cells lying on its boundary. By construction, both the accessibility and the connectivity conditions are sat- isfied, i.e., every sample point is accessible via a straight-line path within its cell, and any two adjacent cells are connected by an edge in the roadmap. Thus, the roadmap provides an exact representation of the planning problem. Once the roadmap is constructed, the explicit cell decomposition is no longer needed, and planning reduces to connecting q S and q G to the roadmap and performing a graph search. Example 4.3.1 (2D Cell Decomposition). Consider the two-dimensional config- uration space in Example 4.3.1. Using vertical decomposition, the free space is partitioned into trapezoids and triangles separated by vertical segments. A motion planning 141 roadmap is obtained by placing representative vertices (shown as black dots) in- side the cells and along their shared boundaries, and connecting them according to adjacency. To solve a motion planning query, start and goal configurations are connected to their respective cells, and a path is found via a standard graph search algorithm. Figure 4.10: Example of a ver- tical cell decomposition in two dimensions.The free space C free is decomposed into trape- zoids, with roadmap vertices (solid dots) placed inside cells and along boundaries. A mo- tion planning query reduces to graph search on this roadmap. 4.3.2 Other Roadmap Construction Methods Beyond cell decomposition, several other methods have been developed to con- struct roadmaps, often exploiting geometric properties of the free space. For instance, maximum clearance roadmaps attempt to maintain as much distance as possible from obstacles, effectively following the âskeletonâ of the free space. A well-known example is the generalized Voronoi diagram, where the roadmap con- sists of points in C free equidistant to at least two obstacles. Such roadmaps have the advantage of producing paths that maximize safety margins, which is par- ticularly useful for robots operating in tight or uncertain environments. Another approach is the shortest path roadmap, which constructs the roadmap by connect- ing pairs of points in C free with the shortest possible paths that avoid obstacles. For an in-depth discussion of these and other roadmap construction techniques, we refer the reader to LaValle [77]. Each of these roadmap constructions provides a different balance between ease of computation, path quality, and robustness. The choice of method depends on the geometry of the robot and environment, as well as the requirements of the task at hand. 142 principles of robot autonomy 4.4 Sampling-based Motion Planning The limitations of combinatorial and grid-based methods, particularly their com- putational complexity in high-dimensional spaces, have motivated the develop- ment of sampling-based motion planning algorithms. At a high level, these algo- rithms explicitly avoid the need for an explicit representation of C free and C obs , instead relying on random sampling to capture the structure of the configuration space. To do so, sampling-based methods combine random sampling in C-space with black-box collision detection algorithms that can determine whether a config- uration or path segment is collision-free (i.e., lies in C free ). By incrementally con- necting such collisionâfree samples, sampling-based methods are able to build a roadmap or tree structure that captures the feasible connectivity of the free space without ever explicitly representing it, in practice ensuring significant computational speed ups. This paradigm offers several advantages: âą Conceptual simplicity: algorithms are relatively easy to understand and imple- ment. âą Generality: the same framework applies to different robots and environments. âą Extensibility: the methodology can be extended beyond purely geometric set- tings to handle kinodynamic planning, differential constraints, and uncer- tainty. At the same time, samplingâbased methods have inherent limitations. Unlike ex- act combinatorial approaches, they generally offer weaker guarantees regarding the existence or quality of the resulting solution. Moreover, it may be challenging to know a priori how many samples are needed to find a solution, or to ensure that the planner will find a solution if one exists. On the theoretical side, most methods offer probabilistic guarantees rather than deterministic ones: they are probabilistically complete (i.e., if a feasible path exists, the probability of finding one approaches 1 as the number of samples n tends to infinity) and, for certain variants, asymptotically optimal (i.e., if C n is the cost of the best path found after n samples and C â is the optimal cost, then C n tends to C â with probability 1). Both properties are asymptotic and probabilistic in nature, as they provide no finite-sample guarantee but only that the probability of failure (or of suboptimal- ity) vanishes as n grows. These properties will be discussed in more detail in Section 4.4.3. Traditionally, sampling-based motion planning algorithms fall into two main approaches: probabilistic roadmaps (PRMs) and rapidly-exploring random trees (RRTs). PRMs are multi-query planners, as their precomputed structures can be reused to answer multiple planning problems in the same environment. By contrast, RRTs are single-query planners, designed to find a solution for a specific startâgoal pair (q S , q G ) in a given free configuration space C free . Despite this distinction, both families of algorithms share two fundamental components. The first is a collision detection routine, CollisionFree(q), which determines whether a configuration q motion planning 143 lies within the free spaceC free . The second is a local planner, LocalPlanner(q, q âČ ), which produces a short path segmentâoften a straight line in configuration space or a dynamically feasible motionâand verifies its validity by checking for colli- sions. Below, we provide an overview of PRMs and RRTs, along with their theoretical properties and practical considerations. 4.4.1 Probabilistic Roadmaps (PRMs) PRMs are among the most influential samplingâbased methods for motion plan- ning. At a high level, PRMs are a multi-query planner that constructs a roadmap in the configuration space by randomly sampling configurations and connecting them to form a graph. The PRM algorithm consists of two main phases: a con- struction phase and a query phase. During the construction phase, the algorithm samples a set of configurations q 1 , q 2 , . . . , q n from C free using a uniform or bi- ased sampling strategy. Each sampled configuration is then used to construct a roadmap that encodes the connectivity of the free space. As a result, once con- structed, the roadmap can be queried to solve multiple planning problems in the same environment by simply attaching start and goal configurations to the roadmap and running a graph search. Formally, the PRM algorithm can be summarized as follows: 1. Construction Phase: (a) Sample n configurationsq 1 , q 2 , . . . , q n fromC and discard those that lie in C obs using the collision detection method CollisionFree(q). (b) Draw an edge between each pair of configurations q i and q j according to a chosen connection ruleâfor example, if they are within a certain distance threshold r, i.e., ||q i â q j || †r, and if the local planner LocalPlanner(q i , q j ) finds a collision-free path between them, e.g., a straight line segment in C-space. 2. Query Phase: given a query (q S , q G ) â C free , connect q S and q G to the nearest nodes in the roadmap using the local planner, and then use a graph search algorithm (e.g., Dijkstraâs or A â ) to find a path from q S to q G through the roadmap. Section 4.4.1 illustrates an example of the PRM algorithm in a simple 2D envi- ronment. Design considerations. The effectiveness of PRM depends on several key design decisions that shape both its computational performance and its ability to cap- ture the connectivity of the free space. A first consideration concerns the sam- pling strategy. The most straightforward approach is to generate configurations uniformly at random in the configuration space and retain only those that lie in C free . While this method provides unbiased coverage, it may perform poorly 144 principles of robot autonomy Figure 4.11: Roadmap generated by the PRM algorithm.Solid dots denote randomly sampled configurations, and the lines connecting these dots represent collision-free connections estab- lished between configurations within a predefined connection radius r. The initial configura- tion q S and the goal configura- tion q G are connected through the roadmap, with the result- ing solution path highlighted by the dashed red line.This path is obtained by applying a graph search algorithm to the constructed roadmap. in environments dominated by narrow passages, since these regions occupy lit- tle volume and are therefore rarely sampled. To address this limitation, various biased sampling strategies have been proposed, which aim to increase the likeli- hood of sampling configurations in challenging areas of the configuration space. More generally, heuristic biasing strategies can direct samples toward regions that are likely to be relevant for specific planning tasks, trading uniformity for effectiveness in complex environments. Another important design choice is the method used to connect sampled con- figurations. Two rules are most common: the fixed-radius rule, where each ver- tex attempts to connect to all samples within a ball of radius r, and the k-nearest neighbor rule, where each vertex connects to its k closest samples. As will be dis- cussed in the remainder of this chapter, both connection rules are supported by theoretical results that tie the values of r and k to the number of samples and the dimensionality of the space (see, e.g., Karaman and Frazzoli [65]). These results ensure that, as the number of samples grows, the roadmap becomes sufficiently connected to capture the topology ofC free without introducing an excessive num- ber of samples Characteristics of PRMs. The probabilistic roadmap framework is particularly well suited to environments that remain static across multiple planning queries. Since the construction of the roadmap can be computationally demandingâdominated by nearest-neighbor queries and collision checkingâit is most effective when the investment in preprocessing can be amortized over many queries posed in the same workspace. In these contexts, the roadmap serves as a reusable data struc- ture that compactly encodes the connectivity of the free space, enabling queries to be answered quickly with standard graph search algorithms. For these reasons, PRM has become a method of choice in domains where multiple planning queries must be solved in high-dimensional but largely static motion planning 145 environments. Its strength lies not in producing a single solution quickly, but in building a reusable structure that captures the topology of the free space and can then be reused to efficiently solve many queries. Overall, PRM-like motion planning algorithms are widely recognized to find âgoodâ paths in practice, even in high-dimensional configuration spaces. How- ever, this may require a large number of expensive collision checks, thus incurring significant computational costs. 4.4.2 Rapidly-Exploring Random Trees (RRTs) RRT is a foundational samplingâbased algorithm designed for single-query mo- tion planning. In contrast to the precomputation strategy of PRMs, RRT incre- mentally grows a tree 10 âdenotedT = (V ,E)ârooted at the initial configuration 10 A tree is a special type of graph that is connected and acyclic. In other words, there is exactly one path between any two vertices in a tree. q S , seeking to connect it to a specified goal configuration q G . As a result, RRT is particularly well suited for scenarios in which only a single query needs to be solved, such as when the environment dynamically changes. RRT proceeds iteratively, where each iteration consists of the following steps: 1. Sample a random configuration q rand from the configuration spaceC. 2. Find the nearest vertex q near in the tree T to the sampled configuration q rand under an appropriate distance metric d(·,·) (e.g., Euclidean distance). 3. Generate a new configuration q new by moving from q near toward q rand , ensur- ing that the motion from q near to q new is collision-free. 4. Update the tree T by adding q new as a new vertex and connecting it to q near with an edge (q near , q new ). The algorithm continues expanding the tree until a vertex is added that is suffi- ciently close to the goal configuration q G 11 . At that point, a path from q S to q G can 11 This proximity can be verified, for example, by attempting to connect the newly added vertex to the goal and checking whether the connection is collision-free.If successful, the algo- rithm terminates. be extracted by backtracking through the tree. If after a budget of N iterations no feasible connection to the goal is found, the algorithm terminates with failure. Section 4.4.2 illustrates an example of the RRT algorithm in a simple 2D envi- ronment. Voronoi bias. A key insight into RRTâs efficiency comes from interpreting its sam- pling dynamics through the lens of Voronoi diagrams (Figure 4.13). Each vertex in the treeT defines a Voronoi cell in configuration space, consisting of all points closer to that vertex than to any other. Since random samples are uniformly distributed, the probability of expanding a given vertex is proportional to the volume of its Voronoi cell. Vertices on the frontier of the tree tend to own large cells, and therefore are more likely to be selected for expansion. This Voronoi bias implicitly drives the tree outward, rapidly exploring uncovered regions without requiring any explicit mechanism for frontier selection. 146 principles of robot autonomy Figure 4.12:Exploration tree generated by the RRT algorithm. Solid dots indicate tree vertices added over iterations. At each step the tree is extended from the nearest vertex toward a ran- dom sample. Figure 4.13: Voronoi bias in RRT. Overall, RRT provides a powerful framework for single-query motion plan- ning. Specifically, RRT-like motion planning algorithms are widely used in prac- tice due to their ability to find feasible paths quickly in high-dimensional spaces. However, the paths produced by RRT are often suboptimal. Its extensions, par- ticularly RRT â and its variants, combine RRTâs exploratory power with provable guarantees on optimality, making them widely used in practical applications. Im- portantly, RRT (as well as PRM) does not require an explicit characterization of C free , which is key to its scalability to higher-dimensionalC spaces. 4.4.3 Theoretical Guarantees Both PRM and RRT algorithms come with theoretical guarantees that underpin their effectiveness in motion planning tasks. Specifically, we consider two key properties which formalize the sense in which planners like PRM, RRT, and their related extensions PRM â and RRT â succeed in the limit of large sampling budgets: probabilistic completeness and asymptotic optimality. Probabilistic completeness. A planner is said to be probabilistically complete if, whenever a feasible path exists, the probability that the algorithm fails to find one approaches zero as the number of samples n tends to infinity. Both PRM and RRT satisfy probabilistic completeness under reasonable assumptions on the configuration space and the sampling process 12 . 12 Intuitively, the sampling distribution must assign nonzero probability to ev- ery open subset of the free space. While an in-depth treatment of probabilistic completeness is beyond the scope of this chapter, we refer the reader to Kavraki et al. [66] and LaValle [78], which establish these guarantees for PRM, RRT, and related algorithms. Asymptotic optimality. A planner is asymptotically optimal if, as the number of samples n tends to infinity, the probability that cost of the best path found by the motion planning 147 algorithm converges to the optimal cost approaches one. Achieving asymptotic optimality requires careful control of the connectivity of the underlying random graph. If connections are too sparse, the graph may fail to capture near-optimal paths. If connections are too dense, computational costs may increase signifi- cantly. Thus, guarantees of asymptotic optimality rely on choosing a connection radius that decreases at an appropriate rate relative to the number of samples to balance these competing effects. Karaman and Frazzoli [65] provided the first rigorous study of asymptotic optimality for sampling-based planners. In particular, they proved the following result for PRM â : Theorem 4.4.1. If the connection radius r(n) satisfies: r(n)â„ Îł log n n 1/d , where d is the dimension of the configuration space and Îł is a constant that depends only on d andC free , then the cost of the best path returned by PRM â after n samples converges to the optimal cost with probability one as nââ. Karaman and Frazzoli [65] also introduced an asymptotically optimal variant of RRT, called RRT â , which incorporates a rewiring step to improve path quality over time. Example 4.4.1 (PRM* in 2D Workspace). Explore ch04/prm _ star.ipynb in the repository github.com/StanfordASL/pora-exercises for an example imple- mentation of the PRM* algorithm that is asymptotically optimal as the number of samples increases. In this example, we consider a simple 2D workspace with some obstacles, and try to plan the shortest path from a start to goal position. Play around with the number of nodes in the PRM to see the optimality and execution time trade-off. 4.4.4 Fast Marching Tree Algorithm (FMT â ) The Fast Marching Tree (FMT â ) algorithm, introduced by Janson et al. [57], is a more recent addition to the family of samplingâbased motion planners. Its key contribution is to achieve asymptotic optimalityâlike PRM â and RRT â âwhile performing dramatically fewer collision checks, often the computational bottle- neck in motion planning. High-level description. FMT â operates on a fixed set of n collisionâfree samples drawn fromC free , together with the initial condition q S , i.e., V =q S âȘq 1 , . . . , q n â C free . Rather than attempting to construct a global roadmap (as in PRM â ) or ex- pand a tree through random sampling (as in RRT â ), FMT â performs graph con- struction and graph search concurrently. At each iteration, it expands the current tree outward in costâtoâarrive space, always advancing from the lowestâcost node in the tree and connecting it to nearby unvisited samples via the best available 148 principles of robot autonomy oneâstep connection. This approach is reminiscent of Dijkstraâs algorithm and of the Fast Marching Method for solving Eikonal equations 13 , in that expanded 13 J. A. Sethian. âA fast marching level set method for monotonically advancing frontsâ. In: Proceedings of the National Academy of Sciences 93.4 (1996), p. 1591â 1595 nodes in the tree never need to be revisited. At any point during the algorithm, the sample set V is partitioned into three disjoint subsets: (i) V open , nodes currently part of the tree and eligible for ex- pansion; (i) V unvisited , samples not yet connected to the tree; and (i) V closed , nodes already expanded and thus excluded from further connections. Initially, V open = q S , V unvisited = V\q S , and V closed =â . At each iteration, the algo- rithm extracts the lowestâcost node q âČ in V open and considers all unvisited neigh- bors q â V unvisited within a radius r n from q âČ . For each such q, it computes the best parent q p â V open minimizing the cost: q â p =arg min q p âN V open (q) h C(q p ) + c(q p , q) i ,(4.6) where C(q p ) is the costâtoâarrive at q p , c(q p , q) is the local path cost, andN V open (q) denotes the set of neighbors of q among the samples in V open . Crucially, only this best candidate edge (q â p , q) is collisionâchecked. If the edge is feasible, q is added to the tree and moved from V unvisited to V open . After all neighbors of q âČ have been processed, q âČ is moved from V open to V closed . The process repeats until either a node in the goal region is added to the tree or V open becomes empty. FMT â âs efficiency stems from its lazy evaluation strategy, such that only one edge per candidate node is collisionâchecked, instead of all possible ones as in PRM â . While this laziness may occasionally discard a better edge, such events occur with vanishing probability as the number of samples increases. Indeed, FMT â is asymptotically optimal and, as n tends to infinity, the cost of the best path returned converges to the optimal cost with probability one. The efficiency gain is substantial, and compared to PRM â , the ratio of collision checks performed by FMT â converges to zero, making it particularly well suited to highâdimensional problems where collision checking dominates computation. In this sense, FMT â occupies a natural middle ground between PRM â and RRT â . Like PRM â , it begins from a fixed set of preâsampled configurations, but instead of forming a global roadmap, it incrementally connects nodes into a tree rooted at the start. Like RRT â , it constructs a single tree, but its expansion enables a more effective exploration and often improved solution quality. 4.4.5 Kinodynamic Planning The geometric motion planning algorithms introduced so far assume no differ- ential constraints on the robotâs motion. This assumption simplifies the planning task, as it allows the problem to be decomposed into two steps: first, computing a collision-free path that ignores dynamics, and then smoothing or reparameter- izing this path so that it can be followed by the robot, as discussed in Chapter 3. However, purely geometric plans are not always straightforward to refine into dy- namically feasible, optimized trajectories, often requiring downstream trajectory optimizers or controllers to perform aggressive corrections. motion planning 149 The kinodynamic motion planning problem extends geometric motion planning by explicitly accounting for a robotâs dynamics. Formally, let X â R n and U â R m denote the state and control spaces of a robotic system, respectively, and consider dynamics of the form: Ìx(t) = f (x(t), u(t)),x(t)âX , u(t)âU .(4.7) As in geometric planning, the robotâs configuration q can be derived from the full dynamic state x, which encodes both position and the information needed for collision checking. The kinodynamic planning problem then seeks to drive the robot from an initial state x init to a goal region X goal while satisfying both collision avoidance and the systemâs dynamical constraints. By embedding kinematic and dynamic constraints at the planning stage, the planner produces nominal motions that are dynamically feasible by construc- tion. This, in turn, provides better initializations for trajectory optimization and reduces the burden on the tracking controller, ultimately improving the overall system performance at execution time. This formulation highlights the fundamental distinction between geometric and kinodynamic planning. In geometric planning, feasible paths are arbitrary collision-free curves in configuration space. In contrast, kinodynamic planning restricts admissible trajectories to those consistent with the systemâs equations of motion. For example, a wheeled robot cannot move sideways, and a quadrotor cannot instantaneously stop due to inertia. These differential constraints drasti- cally alter the search space, as illustrated in Section 4.4.5, where adding dynamics constraints significantly restricts the feasible set of trajectories. Forward-propagation-based algorithms. A practical way to incorporate dynamics into sampling-based planners is through forward propagation. Rather than directly connecting sampled states, the planner samples a control input u â U and a du- ration t, then integrates the system dynamics forward in time to produce a new state. For example, the kinodynamic extension of RRT proceeds by: (i) sampling a random state and finding its nearest neighbor q near in the tree, (i) sampling a random control u â U and propagation time t, and (i) simulating the sys- tem forward from q near to obtain a new state. If the resulting trajectory segment is collision-free, it is added to the tree; otherwise, it is discarded. Notably, un- der mild regularity assumptions on f (x(t), u(t)), variants of RRT with forward propagation have been shown to be probabilistically complete. Kinodynamic planning thus unifies obstacle avoidance with differential con- straints in a single framework, which is particularly important for robotics, where dynamical considerations highly restrict the set of feasible paths, for example in high-speed ground vehicles, aerial robots, or systems with significant momen- tum. 150 principles of robot autonomy (a) Geometric Planning(b) Planning with Dubins Car Dynamics (c) Geometric Planning(d) Planning with Simplified Quadrotor Dynamics Figure 4.14:Comparison of geometricandkinodynamic planning.Obstacles are the solidpolygons/circles(top figures) and solid straight line segments (bottom figures), trees of motion considered by the sampling-basedplannerare shownthroughoutthecon- figuration space, and solution trajectories are highlighted. In each row, the robots start and end in the same configuration, yet the addition of differential constraints in the right-hand figures greatly restricts their possible motions, making the planning problem significantly more challenging.In the top row, enforcing the simple car dynamics prevents the vehi- cle from sliding laterally (b), resulting in a more complex trajectory compared to the ge- ometric case (a). In the bottom row, incorporating momentum through simplified quadrotor dynamics yields a smoother but more circuitous trajectory (d) compared to the geometric case (c), reflecting the constraints imposed by the vehicleâs inertia. [Figure from Schmerling and Pavone [127]]. 4.4.6 Should Probabilistic Planners be Probabilistic? A natural question is whether the success of sampling-based algorithms depends fundamentally on the randomness of the sampling process. In particular, would the theoretical guarantees and practical performance of planners such as PRM â or FMT â still hold if the algorithms were de-randomized, i.e., run on deterministic samples? This question is important for several reasons. Deterministic sampling se- quences could substantially ease certification in safety-critical systems, since they remove probabilistic uncertainty. They would also allow planners to exploit offline computation more effectively, and simplify certain operations such as nearest-neighbor search. Recent results show that the answer is affirmative, with carefully chosen low- dispersion deterministic sampling sequences, one can retain asymptotic opti- mality and completeness guarantees 14 . Thus, randomness is not strictly nec- 14 L.Jansonetal.âDeterministic sampling-based motion planning: Opti- mality, complexity, and performanceâ. In: Int. Journal of Robotics Research 37.1 (2018), p. 46â61 essary for either correctness or performance. Probabilistic sampling provides convenience and generality, but deterministic low-dispersion sequences can yield equally strong guarantees while offering advantages in predictability, efficiency, and ease of certification. motion planning 151 Figure 4.15: A potential field in a 2D configuration space. The attractive potential (top left) cre- ates a basin of attraction at the goal. The repulsive poten- tial (top right) creates barriers around obstacles. The combined potential field (bottom left) de- fines a surface whose negative gradient guides the robot from the start to the goal, resulting in a smooth trajectory (bottom right). 4.5 Potential Field Methods The planning methods described in the previous sections focus on capturing the global connectivity of the robotâs free space in a compact graph representation, which is then searched for a feasible path. In contrast, the approach introduced in this section is based on a fundamentally different idea. Potential field methods 15 15 O. Khatib.âReal-Time Obstacle Avoidance for Manipulators and Mobile Robotsâ. In: The International Journal of Robotics Research 5.1 (1986), p. 90â98 frame the motion planning problem using a physics-inspired analogy, where the robot is treated as a particle moving under the influence of an artificial poten- tial field defined over the configuration space. This field is constructed to guide the robot toward its goal while simultaneously pushing it away from obstacles, resulting in a smooth, continuous path, as illustrated in Section 4.5. The core of this approach is the design of an artificial potential function U(q) : C â R, which is typically composed of two components: an attractive potential U att (q) that pulls the robot toward the goal configuration q goal , and a repulsive potential U rep (q) that pushes it away from obstacles: U(q) = U att (q) + U rep (q).(4.8) The motion of the robot is then determined by following the negative gradient of this total potentialââU(q), which represents the direction of steepest descent in the potential field. Specifically, the artificial force acting on the robot at configu- 152 principles of robot autonomy ration q is given by: F(q) =ââU(q) =ââU att (q)ââU rep (q) =âF att (q)â F rep (q), (4.9) where F att (q) and F rep (q) are the attractive and repulsive forces, respectively. A path is generated by starting at the initial configuration q S and iteratively taking small steps in the direction of this force, effectively performing gradient descent on the potential surface until reaching the goal configuration q G . Below, we describe common choices for the attractive and repulsive potential functions and refer the reader to Latombe [76] for a more comprehensive treat- ment of potential field methods. Attractive potential. The attractive potential is designed to create a basin of attrac- tion around the goal configurationâideally, a unique global minimumâencouraging the robot to move toward it. A common choice for the attractive potential is a parabolic function of the form: U att (q) = 1 2 k att Ï 2 (q, q G ),(4.10) where k att is a positive scaling factor, and Ï(q, q G ) denotes a distance metric (e.g., Euclidean distance) between the current configuration q and the goal configura- tion q G . The function U att (q) is thus positive or null, and attains its minimum value at q G , where U att (q G ) = 0. The resulting attractive force (pointing toward the goal) is given by: F att (q) : =ââU att (q) =âk att Ï(q, q G )âÏ(q, q G ) =âk att (qâ q G ), where Ï(q, q G ) =||qâ q G || andâÏ(q, q G ) = qâ q G ||qâ q G || , (4.11) which is a linear function of the distance to the goal. Repulsive potential. The repulsive potential is designed to create a potential bar- rier around obstacles, preventing the robot from colliding with them. Moreover, it should have a limited range of influence, affecting the robot only when it is within a certain distance from an obstacle. One way to achieve these properties is to define the repulsive potential as: U rep (q) = ïŁ± ïŁČ ïŁł 1 2 k rep 1 Ï(q,C obs ) â 1 Ï 0 2 if Ï(q,C obs )â€ Ï 0 , 0if Ï(q,C obs ) > Ï 0 , (4.12) where k rep is a positive scaling factor, Ï(q,C obs ) denotes the distance from the configuration q to the nearest obstacle in the configuration space, and Ï 0 is a motion planning 153 threshold distance beyond which the repulsive potential has no effect, also re- ferred to as the distance of influence. Formally, the distance to the nearest obstacle is defined as: Ï(q,C obs ) = min q âČ âC obs Ï(q, q âČ ),(4.13) whereC obs is the set of configurations that result in collisions with obstacles. If C obs is a convex region, Ï(q,C obs ) is differentiable everywhere in C free , and the resulting repulsive force is given by: F rep (q) : =ââU rep (q) = ïŁ± ïŁČ ïŁł k rep 1 Ï(q,C obs ) â 1 Ï 0 1 Ï 2 (q,C obs ) âÏ(q,C obs )if Ï(q,C obs )â€ Ï 0 , 0if Ï(q,C obs ) > Ï 0 . (4.14) Specifically, let q â obs be the point on the boundary of the obstacle closest to q, i.e., q â obs = arg min q âČ âC obs Ï(q, q âČ ). Then, the gradient âÏ(q,C obs ) is a unit vector pointing away fromC obs and supported by the line segment connecting q â obs to q. Advantages and disadvantages. The primary advantage of potential field methods is their simplicity and computational efficiency. These methods were originally developed as an online collision avoidance strategy for mobile robots 16 , appli- 16 O. Khatib.âReal-Time Obstacle Avoidance for Manipulators and Mobile Robotsâ. In: The International Journal of Robotics Research 5.1 (1986), p. 90â98 cable in dynamic environments where obstacles may not be known in advance. Instead of performing a complex search over a graph or constructing a roadmap, path generation simply involves evaluating the potential function and its gradient at the robotâs current configuration. However, since potential field planners essentially act as gradient descent algo- rithms on the potential function, they are susceptible to local minima. Specifically, the planner can become trapped in regions where the attractive and repulsive forces balance out, resulting in a net force of zero at a configuration that is not the goal. This issue commonly occurs in environments with concave obstacles (e.g., a U-shaped trap) or in narrow corridors where the repulsive forces from opposing walls can cancel the attractive pull toward the goal. Because of this, standard potential field planners are not complete and may fail to find a path even when one exists. Several techniques have been proposed to address the local minima problem. One approach is to introduce a random âjiggleâ (e.g., a small random pertur- bation to the robotâs configuration) to escape the basin of a local minimum. A more theoretically grounded solution involves constructing special potential fields called navigation functions, which are provably free of local minima except for the goal [122]. However, constructing such functions is computationally ex- pensive and generally only feasible for simple environments. Another related approach, the wavefront planner or brushfire algorithm, discretizes the configuration space (similar to grid-based methods) and propagates a potential wave outward from the goal, effectively creating a potential field on a grid that is guaranteed to be free of local minima. 154 principles of robot autonomy In summary, potential field methods provide a fast and reactive framework for local motion planning and obstacle avoidance. While their simplicity is appealing, their unreliability for global planning due to the local minima problem limits their use as a standalone, complete motion planner. 4.6 Summary In this chapter, we introduced motion planning as the critical link that translates high-level task specifications into feasible, collision-free paths. We formulated the core problem as finding a sequence of actions that drives a robot from a start con- figuration to a goal configuration while avoiding obstacles. A central theme was the abstraction of motion into the robotâs configuration space (C-space), where planning reduces to finding a continuous path in the free space,C free . We began by discussing grid-based methods, which discretize the configuration space and cast the problem as a shortest-path search on a graph. This allowed us to introduce a general family of label-correcting algorithms, including Dijkstraâs algorithm for finding shortest paths, and its informed extension, A*, which uses heuristics to guide the search efficiently toward the goal. We then turned to com- binatorial planning, an exact approach that constructs a roadmap by decomposing the continuous free space into simple cells, guaranteeing completeness but fac- ing computational challenges in high-dimensional spaces. Next, we discussed sampling-based methods, which avoid the explicit construction of the free space by relying on random sampling and collision detection to incrementally build C free . We presented the PRM algorithm, a multi-query planner that builds a reusable graph of the free space, and the single-query RRT algorithm, which incremen- tally grows a tree from the start configuration. We summarized their key theoret- ical properties, including probabilistic completeness and asymptotic optimality, which motivated advanced planners like PRM*, RRT* and the FMT* algorithm. We further extended these concepts to kinodynamic planning, which incorporates system dynamics, and discussed how deterministic low-dispersion sampling can provide similar theoretical guarantees as probabilistic methods while offering benefits in predictability and certification. Finally, we introduced potential field methods, which generate paths by defining artificial potential functions that at- tract the robot to the goal while repelling it from obstacles, enabling fast and reactive local planning at the cost of potential local minima. To learn more. For a deeper exploration of the topics covered in this chapter, sev- eral key resources are available. For a comprehensive and foundational treatment of motion planning, from configuration spaces to combinatorial and sampling- based algorithms, the textbook by LaValle [77] is an essential reference. An in- depth presentation of grid-based planning methods and shortest path algorithms is provided in Bertsekas [12]. A detailed treatment of the D* algorithm for plan- ning in dynamic environments can be found in Stentz [141]. The seminal work on asymptotic optimality for sampling-based planners, which introduced PRM* motion planning 155 and RRT*, is presented in Karaman and Frazzoli [65]. For an in-depth discussion of the FMT* algorithm, we refer the reader to Janson et al. [57]. Kinodynamic planning is thoroughly discussed in Schmerling and Pavone [127]. The exten- sion of performance guarantees to deterministic sampling patterns is explored in Janson et al. [56]. The original formulation of potential field methods for robot motion planning was introduced by Khatib [68], and a comprehensive discussion can be found in Latombe [76]. Finally, for a more modern review of motion plan- ning techniques and their applications in robotics, we recommend the survey by Hauser [49]. 4.7 Exercises The starter code for the exercises provided below is available online through GitHub. To get started, download the code by running in a terminal window: git clone https://github.com/StanfordASL/pora-exercises.git We denote Problems requiring hand-written solutions and coding in Python withand, respectively. Problem 1: A* motion planning In this exercise, you will implement the A* grid-based motion planning algorithm for some simple two-dimensional environments. In the files ch04/exercises/a _ star.ipynb and ch04/exercises/a _ star.py, you will im- plement the key parts of the A* algorithm (see Algorithm 4.2), run the algorithm on some randomly generated path planning problems, and then explore a way to smooth the resulting discrete paths, which could be useful for practical robot motion planning tasks. In this implementation of A*, we will represent the free space by a graph, which is traversed by sampling and collision-checking states from a determin- istic grid. This implementation can be categorized as informed, deterministic sampling-based planning (âinformedâ due to the A* heuristic). 1. Implement the remaining functions in a _ star.py within the Astar class. These functions represent many of the key functional blocks at play in motion plan- ning algorithms: âą is _ free which checks whether a state is collision-free and valid. âą distance which computes the travel distance between two points. âą get _ neighbors which finds the free neighbor states of a given state. âą solve which runs the A* motion planning algorithm. Note: Notice that we collision-check states but do not collision-check edges. This saves us some computation (collision-checking is often one of the most 156 principles of robot autonomy Algorithm 4.2: A* Motion Planning Data: Start node q S , goal node q G Result: Shortest path from q S to q G (if reachable) O âq S / * Initialize open set * / C â / * Initialize closed set * / C(q S )â 0 / * Initialize cost-to-arrive * / Ì C(q S )â distance(q S , q G ) / * Initialize estimated cost * / whileO is not empty do qâ lowest_est_cost_through(O) if q = q G then return reconstruct_path() O.remove(q) C.add(q) for q âČ â free_neighbors(q) do if q âČ âC then continue if q âČ ÌžâO then O.add(q âČ ) else if C(q) + distance(q, q âČ ) > C(q âČ ) then continue parent(q âČ )â q C(q âČ )â C(q) + distance(q, q âČ ) Ì C(q âČ )â C(q âČ ) + distance(q âČ , q G ) return Failure expensive operations in motion planning). Also, in this case the obstacles are aligned with the grid, so paths will remain collision-free. However, outside such special circumstances we should add edge collision-checking and/or inflate ob- stacles to guarantee collision-avoidance. 2. Planning a path on a grid is often not very desirable for a real robot that would have to track the trajectory. In this exercise, we will smooth the paths from A* by fitting a cubic spline to the path nodes. Implement this within the compute _ smooth _ plan function of a _ star.ipynb. Note: There are many ways to ensure smoothed solutions are collision-free (for example, collision-checking smoothed paths and running a dichotomic search on the smoothing parameters to find a tight fit against obstacles, or inflating obstacles in the original planning to give additional room for smoothing). This strategy can be used on geometric sampling-based planning methods as well. Problem 2: Rapidly-exploring random trees In this exercise, you will implement the RRT sample-based motion planning al- gorithm to plan paths in simple 2D environments. In the files motion planning 157 ch04/exercises/rrt.ipynb and ch04/exercises/rrt.py, you will implement the key parts of the RRT algorithm and define a GeometricRRT planner that leverages simple straight line connections between nodes. For this implementation of RRT, we consider a âGeometricâ RRT problem where nodes are connected with simple straight lines. Algorithm 4.3: RRT Motion Planning Data: Start node q S , goal node q G , max iterations N max , goal sampling bias α Result: Path from q S to q G (if reachable) T âq S / * Initialize tree * / for kâ 1 to N max do if rand() < α then q âČ â q G else q âČ â random_sample() qâ nearest_neighbor(q âČ ,T ) q âČ â steer_towards(q, q âČ ) if is_free_motion(q, q âČ ) then parent(q âČ )â q T .add(q âČ ) if q âČ = q G then return reconstruct_path() return Failure 1. Implement the remaining functions in rrt.py within the RRT and GeometricRRT classes: âą RRT.solve which runs the RRT algorithm in Algorithm 4.3. âą GeometricRRT.nearest _ neighbor which computes the nearest neighbor in the current tree to a given point using Euclidean distance. âą GeometricRRT.steer _ towards to compute a new state from a target state following a straight line path. 2. Implement the function RRT.shortcut _ path to try to find a shorter path from the existing tree by removing nodes from the path that arenât strictly needed. Run the code in rrt.ipynb to check your work. 3. Run the provided code (RRT.solve _ optimal) to compare the standard RRT algorithm you implemented against the RRT* algorithm. 158 principles of robot autonomy References [12]D. Bertsekas. Dynamic Programming and Optimal Control. Athena Scientific, 2000. [49]K. Hauser. âMotion and Path Planningâ. In: Encyclopedia of Robotics. Springer, 2020, p. 1â11. [56]L. Janson et al. âDeterministic sampling-based motion planning: Optimal- ity, complexity, and performanceâ. In: Int. Journal of Robotics Research 37.1 (2018), p. 46â61. [57]L. Janson et al. âFast Marching Tree: A Fast Marching Sampling-Based Method for Optimal Motion Planning in Many Dimensionsâ. In: Int. Jour- nal of Robotics Research 34.7 (2015), p. 883â921. [65]S. Karaman and E. Frazzoli. âSampling-based Algorithms for Optimal Motion Planningâ. In: Int. Journal of Robotics Research 30.7 (2011), p. 846â 894. [66]L. E. Kavraki et al. âProbabilistic roadmaps for path planning in high- dimensional configuration spacesâ. In: IEEE Transactions on Robotics and Automation 12.4 (1996), p. 566â580. [68]O. Khatib. âReal-Time Obstacle Avoidance for Manipulators and Mobile Robotsâ. In: The International Journal of Robotics Research 5.1 (1986), p. 90â 98. [76]J. C. Latombe. Robot Motion Planning. USA: Kluwer Academic Publishers, 1991. [77]S. M. LaValle. Planning Algorithms. Cambridge, U.K.: Cambridge Univer- sity Press, 2006. [78]S. M. LaValle. Rapidly-Exploring Random Trees: A New Tool for Path Planning. 1998. [122]E. Rimon. Exact robot navigation using artificial potential functions. PhD The- sis. Yale University, 1990. [127]E. Schmerling and M. Pavone. âKinodynamic Planningâ. In: Encyclopedia of Robotics. First. Springer, 2019. [128]J. A. Sethian. âA fast marching level set method for monotonically advanc- ing frontsâ. In: Proceedings of the National Academy of Sciences 93.4 (1996), p. 1591â1595. [141]A. Stentz. âThe focussed D* algorithm for real-time replanningâ. In: 14th International Joint Conference on Artificial Intelligence. 1995, p. 1652â1659. Part I Robot Perception This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 5 Introduction to Robot Sensors The three main pillars of robotic autonomy are perception, planning, and control, which correspond to the see, think, and act stages of autonomy. The perception component consists of the numerous challenges associated with a robot sensing and understanding its environment, and a key element of perception is the sen- sors the robot uses to extract meaningful information about the world. In the next few chapters, we focus on the robot perception problem, and in particular we introduce common sensors utilized in robotics applications, discuss their key performance characteristics, and describe strategies for extracting useful informa- tion from the sensor measurements 1 . 1 G. Dudek and M. Jenkin. âInertial Sen- sors, GPS, and Odometryâ. In: Springer Handbook of Robotics.Springer, 2008, p. 477â490, R. Siegwart, I. R. Nour- bakhsh, and D. Scaramuzza. Introduction to Autonomous Mobile Robots. MIT Press, 2011 Robots operate in diverse environments which often require diverse sets of sensors for effective perception. For example, a self-driving car may utilize cam- eras, lidar, and radar for detecting objects in the environment. It also requires sensors for characterizing the physical state of the vehicle itself, such as inertial measurement units (IMU), GNSS positioning sensors 2 , and more. In this chapter, 2 Global Navigation Satellite System we will begin by introducing the different types of sensors in Section 5.1. Next, we will discuss the performance characteristics of sensors in Section 5.2, and then we will discuss common errors and uncertainty quantification for them in Sec- tion 5.3. Finally, we will survey some of the most common sensors used in mobile robotics applications in Section 5.4. 5.1 Sensor Classifications We use the terms proprioceptive and exteroceptive to distinguish between sensors that measure the environment and sensors which measure quantities related to the robot itself. Definition 5.1.1 (Proprioceptive). Proprioceptive sensors measure values internal to the robot. For example, a proprioceptive sensor might measure motor speed, wheel load, robot arm joint angles, or battery voltage. Definition 5.1.2 (Exteroceptive). Exteroceptive sensors acquire information from the robotâs environment. For example, exteroceptive sensors measure distances to objects, light intensity, and sound amplitude. This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 162 principles of robot autonomy Generally speaking, exteroceptive sensor measurements are more likely to re- quire interpretation by the robot in order to extract meaningful environmental features. In addition to characterizing sensors based on what they measure, we also characterize sensors as passive or active based on how they operate. Definition 5.1.3 (Passive sensor). Passive sensors, such as thermometers and cam- eras, measure ambient environmental energy entering the sensor. Definition 5.1.4 (Active sensor). Active sensors, such as ultrasonic sensors, lidar and radar, emit energy into the environment and measure the reaction. Classifying a sensor as active or passive is important because each exhibits unique characteristics and challenges. For example, passive sensors are heavily influenced by environmental conditions, such as a cameraâs reliance on good ambient lighting to take quality images. 5.2 Sensor Performance Different types of sensors exhibit varying performance attributes. While some sensors maintain exceptional accuracy in controlled laboratory settings, their per- formance may suffer in natural real-world environments. Conversely, other sen- sors offer narrow, high-precision data across a variety of settings. We quantify and compare sensor performance characteristics by defining metrics related to design specifications and in situ 3 performance. 3 In situ metrics quantify how well a sen- sor performs in the real environment. 5.2.1 Design Specification Metrics A number of performance characteristics are specifically considered when design- ing a sensor, and which are also used to quantify its overall nominal performance capabilities. 1. Dynamic range quantifies the ratio between the lower and upper limits of the sensor inputs under normal operation. We usually express this metric in deci- bels (dB), and compute it as: DR = 10 log 10 (r) [dB], where r is the ratio between the upper and lower limits. In addition to the dynamic range ratio, the actual range is also an important sensor metric. For example, an optical rangefinder has a minimum operating range and gives spurious data when measurements are taken with the object closer than that minimum. 2. Resolution is the minimum difference between two values that can be detected by a sensor. The lower limit of the dynamic range of a sensor is usually equal to its resolution 4 . 4 This is not necessarily the case for dig- ital sensors 3. Linearity characterizes whether or not the sensorâs output depends linearly on the input. introduction to robot sensors 163 4. Bandwidth or frequency is used to measure the speed with which a sensor can provide a stream of readings. We usually express this metric in units of Hertz (Hz), which is measurements per second. High bandwidth sensors are de- sirable so that downstream information can be updated at a high rate. For example, mobile robots may have to limit their maximum speed based on the bandwidth of their obstacle detection sensors. 5.2.2 In Situ Performance Metrics Metrics related to the design specifications can be reasonably quantified in a lab- oratory environment and then extrapolated to predict performance during real- world deployment. However, several important sensor metrics cannot be ade- quately characterized in lab settings since they are influenced by complex inter- actions between the environment. 1. Sensitivity defines the ratio of change in the output from the sensor to a change in the input. High sensitivity is often undesirable because any noise to the input can be amplified, but low sensitivity might degrade the ability to extract useful information from the sensorâs measurements. Cross-sensitivity defines the sensitivity to environmental parameters that are unrelated to the sensorâs target quantity. For example, a flux-gate compass can demonstrate high sen- sitivity to magnetic north and is therefore useful for mobile robot navigation. However, the compass also has high sensitivity to ferrous building materials, so much so that its cross-sensitivity often makes the sensor useless in some in- door environments. High cross-sensitivity of a sensor is generally undesirable, especially when it cannot be modeled. 2. Error of a sensor is defined as the difference between the sensorâs output mea- surements and the true values being measured, within some specific operating context. Given a true value, v, and a measured value, m, we define the error as e : = mâ v. 3. Accuracy is defined as the degree of conformity between the sensorâs measure- ment and the true value, and is often expressed as a proportion of the true value, for example we may state that a sensor has 97.5% accuracy. Therefore, small error corresponds to high accuracy and large error corresponds to low accuracy. For a measurement, m, and true value, v, we define the accuracy as a : = 1â|mâ v|/v. Characterizing sensor accuracy is challenging since obtaining the true value, v, can be difficult or impossible. 4. Precision defines the reproducibility of the sensor results. For example, a sen- sor has high precision if multiple measurements of the same environmental quantity are similar. It is important to note that precision is not the same as accuracy 5 . 5 A very precise sensor can still be highly inaccurate 164 principles of robot autonomy 5.3 Sensor Errors and Uncertainty Modeling When discussing in situ performance metrics such as accuracy and precision, it is important to be able to reason about the sources of sensor errors. In particular, it is important to distinguish between two main types of error, systematic errors and random errors. 1. Systematic errors are caused by factors or processes that can in theory be mod- eled because they are deterministic and therefore reproducible and predictable. Calibration errors are a common source of systematic errors in sensors. 2. Random errors cannot be predicted using a sophisticated model since they are stochastic and unpredictable. Hue instability in a color camera, spuri- ous rangefinding errors, and black level noise in a camera are all examples of random errors. To reliably employ a sensor in practice, it is beneficial to characterize its sys- tematic and random errors to allow for corrections that improve its accuracy and provide information about its precision. We refer to the process of quantifying sensor errors and identifying their origins as error analysis. This analysis often entails identifying all sources of systematic errors, modeling random errors 6 , and 6 For example, using Gaussian distribu- tions. assessing the cumulative effect of errors on the sensorâs output. However, conducting a comprehensive error analysis can be difficult due to several factors. A significant challenge arises due to a blurring between system- atic and random errors that is the result of changes to the operating environment. For instance, exteroceptive sensors on a mobile robot face varying measurement conditions as the robot navigates, with the sensorâs performance potentially in- fluenced by the robotâs own movement. Therefore, while we can classify sensor errors as systematic or random in controlled environments, accurately character- izing these errors becomes substantially more complex in real-world settings. If we could perfectly model and understand all systematic errors in sensor measurements we could theoretically correct for them. However, in practice, this is often not feasible. We therefore characterize uncertainty due to random errors by using probability distributions. Given the practical challenge of identifying all sources of random error, we commonly make assumptions when modeling the error distribution. We commonly assume that random errors have a zero-mean, and that the distribution is symmetric and unimodal 7 . These assumptions can 7 A very common distribution that fits these properties is the Gaussian distri- bution. make mathematical analysis easier, but they also have limitations. For example, some assumptions, such as the unimodality of the distribution, may not hold true in real-world applications. Example 5.3.1 (Sensor uncertainty assumptions). Consider a sonar sensor, an ac- tive sensor that uses acoustic pulses to measure distance. Suppose the sonarâs accuracy is high, with random errors mainly stemming from noise from internal timing circuits. We could reasonably assume this noise is unimodal and possibly Gaussian. However, in scenarios where the sonar encounters materials causing introduction to robot sensors 165 coherent reflections, distance over-estimations become likely and could result in a bias towards positive errors. A comprehensive distribution that also captures this effect should be bimodal and asymmetric. 5.4 Common Sensors in Mobile Robotics In mobile robotics applications, we encounter both proprioceptive and exterocep- tive sensors working together to enable autonomous operation. Proprioceptive sensors provide the robot with information about its internal stateâencoders measure joint positions and wheel rotations, IMUs track orientation and acceler- ation, and heading sensors determine the robotâs direction. These sensors form the foundation for understanding the robotâs own configuration and motion. Ex- teroceptive sensors, on the other hand, gather information about the surround- ing environmentâactive ranging sensors measure distances to obstacles, beacons provide absolute position references, and vision sensors capture rich visual in- formation about the scene. The following sections examine key examples of each type, with particular emphasis on those most commonly used in mobile robotics. 5.4.1 Proprioceptive Sensors Encoders. Encoders are proprioceptive electro-mechanical sensors that convert mechanical motion into a series of digital signals that can be interpreted to mea- sure relative or absolute position measurements 8 . One common application of 8 Thanks to their extensive use across many domains,significant advance- ments have been made in developing af- fordable encoders that provide high res- olution. encoders in robotics is for sensing the rotation angle and speed of wheels or mo- tors. This is important for being able to design good control laws for wheel speed control and motor-driven joints. One common type of encoder is the optical encoder. Optical encoders work by directing light through slits in a rotating metal or glass disc onto a photodiode, creating sine or square wave pulses corresponding to the discâs rotation. We can then integrate the number of wave peaks to determine how much the disc has rotated. The encoderâs resolution, expressed in cycles per revolution (CPR), de- termines its minimum angular resolution. In terms of bandwidth, it is critical that the encoder is sufficiently fast to handle the expected shaft rotation rates 9 . 9 Encoder bandwidth is generally not a concern in mobile robot applications. Quadrature encoders are also common in robotics applications to additionally sense the direction of rotation. As with most proprioceptive sensors, encoders typically operate in a very predictable and controlled environment and we can account for their systematic errors and cross-sensitivities. In practice, we often as- sume perfect accuracy of optical encoders since their errors are typically dwarfed by errors in downstream components. Inertial measurement unit (IMU). Inertial measurement units (IMU) are devices that use gyroscopes and accelerometers to estimate relative position, orienta- tion, velocity, and acceleration with respect to an inertial reference frame. An accelerometer measures net acceleration due to external forces, including gravity, 166 principles of robot autonomy while a gyroscope measures angular velocity. Modern IMUs, such as those in mo- bile phones, typically use Micro Electro-Mechanical Systems (MEMS) technology for both components. We show the general working principle of an IMU in Section 5.4.1. First, we in- tegrate gyroscope data to estimate the vehicle orientation while three accelerom- eters estimate the instantaneous acceleration along each axis. We then transform the acceleration into the local navigation frame using the current estimate of the vehicle orientation relative to gravity and subtract the gravity vector from the measurement. Next, we integrate the resulting acceleration to obtain the velocity and integrate again to compute the position, provided that we know both the initial velocity and position. Rate gyroscope Integrate (to orientation) Accelerometer Transform to local frame Gravity correction Integrate (to velocity) Initial velocity Integrate (to position) Initial position Acceleration Velocity Position Figure 5.1: Inertial measurement unit (IMU) block diagram. One of the fundamental issues with IMUs is the phenomenon called drift, which describes the slow accumulation of errors over time. Drift in any one component will also affect the downstream components. For example, drift in the gyroscope leads to errors in the estimation of the vehicle orientation relative to gravity, which results in incorrect cancellation of the gravity vector. Addition- ally, errors in acceleration measurements will cause the integrated velocity to drift in time, which will in turn also cause position estimate drift. We can account for drift by using periodic references to some external measurement, such as GNSS position measurements, cameras, or other sensors. Heading sensors. Heading sensors determine the robotâs orientation in space and can be proprioceptive (gyroscopes) or exteroceptive (compasses). Compasses measure the Earthâs magnetic field to provide an estimate of direction. Digital compasses using the Hall effect are inexpensive but often suffer from poor res- olution and accuracy, while flux-gate compasses have improved resolution and accuracy but are more expensive. Both types are vulnerable to magnetic field disturbances, making them less suitable for indoor applications. Gyroscopes pre- serve their orientation with respect to a fixed inertial reference frame and can be mechanical or optical. While high-quality gyroscopes can achieve excellent accu- introduction to robot sensors 167 racy (angular drift of about 0.1 degrees in 6 hours), they are expensive and still suffer from drift over time. 5.4.2 Exteroceptive Sensors Active ranging. Active ranging sensors provide direct distance measurements to objects in the vicinity of the sensor. These sensors are important in robotics for localization and environment reconstruction. There are two main types of active ranging sensors: time-of-flight active ranging sensors and geometric active ranging sensors 10 . 10 Examples of time-of-flight sensors in- clude ultrasonic, laser rangefinder, and time-of-flight cameras, and examples of geometric sensors include optical trian- gulation and structured light sensors. 1. Time-of-flight Active Ranging: Time-of-flight active ranging sensors make use of the propagation speed of sounds or electromagnetic waves. In particular, the travel distance is given by: d = ct 2 , where d is the distance to the target, c is the speed of wave propagation, and t is the measured round-trip time of flight. Note that the time of flight is signif- icantly smaller when using electromagnetic signals, on the order of nanosec- onds for distances on the order of meters, which can make these types of sensors more challenging to develop in an affordable and robust way. The quality of time-of-flight range sensors depends on several factors including uncertainties in determining the exact time of arrival of the reflected signal, inaccuracies in the time of flight measurement, the dispersal cone of the trans- mitted beam 11 , interaction with the target 12 , and the speed of the mobile robot 11 Mainly with ultrasonic range sensors. 12 For example, surface absorption, spec- ular reflections. and dynamic targets. 2. Geometric Active Ranging: Geometric active ranging sensors use geometric prop- erties in the measurements to establish distance readings. Generally, these sen- sors project a known pattern of light and then we can use geometric properties to analyze the reflection and estimate range via triangulation. Optical triangu- lation sensors (1D) transmit a collimated beam toward the target and use a lens to collect reflected light and project it onto a position-sensitive device or linear camera. Structured light sensors (2D or 3D) project a known light pattern such as a point, line, or texture, onto the environment. The reflection is captured by a receiver and then, together with known geometric values, we can estimate range via triangulation. Beacons. Beacons are signaling devices with precisely known positions 13 that 13 Stars and lighthouses are classic exam- ples. enable position determination through relative measurements. The GNSS po- sitioning system is an advanced example of beacons that works by processing synchronized signals from at least four satellites to estimate three position co- ordinates and a clock correction variable. Indoor positioning systems often use camera-based motion capture or ultra-wideband beacons for similar functionality in GPS-denied environments. 168 principles of robot autonomy Vision sensors. Vision sensors have become crucial for perception in robotics due to their ability to capture an enormous amount of information about the environ- ment 14 . Unlike the sensors discussed above which provide specific measurements 14 The human eye provides millions of bits of information per second. like distance or orientation, cameras capture rich visual data that can be pro- cessed to extract various types of information including object detection, depth estimation, motion tracking, and scene understanding. The main challenges as- sociated with vision-based sensing are related to processing digital images to extract salient information like object depth, motion and object detection, color tracking, feature detection, scene recognition, and more. We generally refer to the analysis and processing of images as computer vision and image processing. The next chapter will explore camera models and calibration in detail, followed by techniques for extracting 3D information from visual data. 5.5 Summary This chapter introduced the fundamental concepts of robot sensors, including their classifications, performance metrics, and common types used in mobile robotics. Understanding sensor characteristicsâincluding their limitations and error sourcesâis essential for effective robot perception. While weâve surveyed various sensor types, the remainder of this part will focus specifically on vision- based sensing, beginning with camera models and calibration in the next chapter. Later chapters will introduce how these sensors can be used to extract useful information about the environment, such as object detections. To learn more. To dive deeper into robot sensors and perception, Introduction to Autonomous Mobile Robots by Siegwart, Nourbakhsh, and Scaramuzza [135] pro- vides a comprehensive overview of various sensor types and their applications in robotics. Additionally, readers interested in robotic sensor details are encouraged to read Dudek and Jenkin [33]. introduction to robot sensors 169 References [33]G. Dudek and M. Jenkin. âInertial Sensors, GPS, and Odometryâ. In: Springer Handbook of Robotics. Springer, 2008, p. 477â490. [135]R. Siegwart, I. R. Nourbakhsh, and D. Scaramuzza. Introduction to Au- tonomous Mobile Robots. MIT Press, 2011. 6 Camera Models and Calibration While the previous chapter surveyed various sensor types used in robotics, vi- sion sensors warrant special attention due to their unique ability to capture rich, high-dimensional information about the environment. A single camera image can contain information about object identity, pose, color, texture, and spatial relationshipsâinformation that would require multiple specialized sensors to obtain otherwise. However, extracting useful information from camera images requires understanding how three-dimensional scenes are projected onto two- dimensional image planes, and how we can calibrate cameras to enable accurate measurements. This chapter explores the mathematical foundations of camera models, from the basic pinhole model to modern RGB-D sensors, and presents practical meth- ods for camera calibration that enable quantitative vision-based perception. In Section 6.1 we introduce the principles of image formation and the mathematical models that describe how cameras capture images. Next, in Section 6.2, we derive the perspective projection equations and discuss practical considerations such as lens distortion. Then, we will discuss an increasingly popular type of camera, RGB-D cameras, in Section 6.3, which provide both color and depth information. Finally, in Section 6.4 and Section 6.5, we derive the mathematical model for per- spective projection and present methods for camera calibration, which is essential for using these models in practice. 6.1 Digital Cameras and Image Formation Modern cameras consist of a sensor that captures light and converts the resulting signal into a digital image. Light falling on an imaging sensor is usually picked up by an active sensing area, integrated for the duration of the exposure 1 , and 1 The duration of exposure is usually ex- pressed as the shutter speed, such as 1/125, 1/60, or 1/30 of a second. then passed to a set of sense amplifiers. 6.1.1 Image Sensors The two main kinds of sensors used in digital cameras today are charge-coupled devices (CCD) and complementary metal-oxide-semiconductor (CMOS) sensors. This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 172 principles of robot autonomy A CCD chip is an array of light-sensitive picture elements called pixels, and can contain between 20,000 and several million pixels total. We can think of each pixel as a light-sensitive discharging capacitor that is 5 to 25ÎŒm in size. While complementary metal oxide semiconductor (CMOS) chips also consist of an ar- ray of pixels, they are quite different from CCD chips. In particular, along the side of each pixel are several transistors specific to that pixel. CCD sensors have typically outperformed CMOS for quality-sensitive applications such as digital single-lens-reflex cameras, while CMOS sensors are better for low-power appli- cations. However, today, CMOS sensors are standard in most digital cameras. 6.1.2 Image Formation Rays of light reflected by an object tend to be scattered in many directions and may consist of different wavelengths. Averaged over time, the emitted wave- lengths and directions for a specific object can be precisely described using object- specific probability distribution functions. In particular, the light reflection prop- erties of a given object are the result of how light is reflected, scattered, or ab- sorbed based on the objectâs surface properties and the wavelength of the light. For example, an object might look blue because blue wavelengths of light are primarily scattered off the surface while other wavelengths are absorbed. Cameras capture images by sensing reflected light rays on a photoreceptive surface such as a CCD or a CMOS sensor. Since light reflecting off an object is generally scattered in many directions, exposing a planar photoreceptive surface to these reflected rays would result in many rays being captured at each pixel, which would lead to blurry images. A solution to this issue is to add a barrier in front of the photoreceptive surface that only lets some of the rays pass through an aperture, as we show in Section 6.1.2. The earliest approach to filtering light rays in this way was to have a small hole in the barrier surface. We refer to cameras with this type of filter as pinhole cameras. Figure 6.1: Light rays on a pho- toreceptive surface, referred to as the image plane. On the left, numerous rays being reflected and scattered by the object leads to blurry images whereas, on the right, a barrier has been added so that the scattered light rays can be distinguished. 6.2 Camera Models 6.2.1 Pinhole Camera Model A pinhole camera has no lens but rather a single small aperture. Light from the scene passes through this pinhole aperture and projects an inverted image onto camera models and calibration 173 the image plane, as we show in Section 6.2.1. While modern cameras do not operate in this way, we can use the principles of the pinhole camera to derive useful mathematical models. Figure 6.2:Pinhole camera model. Due to the geometry of the pinhole camera system, the objectâs image is inverted on the image plane. In this figure, O is the camera center, c is the im- age center, and p is the principal point. We start by defining several useful references to help develop the mathematical pinhole camera model. First, the camera reference frame is centered at a point, O, that is at a focal length, f , in front of the image plane, as we show in Section 6.2.1. We define this reference frame, with directions ( i, j, k ) , such that the k axis is coincident with the optical axis that points toward the image plane. We denote the coordinates of a point in the camera frame by P = ( X, Y, Z ) . When a ray of light is emitted from a point, P, and passes through the pinhole at point O, it gets captured on the image plane at a point p. Since these points are all collinear, we can deduce the following relationships between the coordinates P = ( X, Y, Z ) and p = ( x, y, z ) : x = λX,y = λY,z = λZ, for some λâ R. This leads to the relationship: λ = x X = y Y = z Z . From the geometry of the camera, we can see that z = f where f is the focal length, such that we can rewrite these expressions as: x = f X Z ,y = f Y Z .(6.1) Therefore, we can compute the position of the pixel on the image plane that captures a ray of light from the point P. 6.2.2 Thin Lens Model One of the main issues with having a fixed pinhole aperture is that there is a trade-off associated with the apertureâs size. A large aperture allows a greater number of light rays to pass through, which leads to image blurring. A small 174 principles of robot autonomy aperture lets through fewer light rays, but the resulting image is darker. As a solution, lenses focus light by refraction and can be used to replace the aperture, avoiding the need for these trade-offs. We can develop a mathematical model for lenses similar to the pinhole model by using properties from Snellâs law. Section 6.2.2 shows a diagram of the most basic lens model, which is the thin lens model 2 . Snellâs law states that rays passing 2 The thin lens model assumes no optical distortion due to the curvature of the lens. through the center of the lens are not refracted, and those that are parallel to the optical axis are focused on the focal point, labeled F âČ . In addition, all rays passing through P are focused by the thin lens on the point p. We develop a mathematical model similar to Equation (6.1) from the geometry of similar triangles: y Y = z Z , y Y = zâ f f = z f â 1,(6.2) where again the point P has coordinates ( X, Y, Z ) , its corresponding point, p, on the image plane has coordinates ( x, y, z ) , and f is the focal length. Combining these two equations yields the thin lens equation: 1 z + 1 Z = 1 f .(6.3) Note that in this model, and for a particular focal length, f , a point, P, is only in sharp focus if the image plane is located a distance z from the lens. In practice, an acceptable focus is possible within some range of distances referred to as depth of field or depth of focus. Additionally, if Z approaches infinity, light would focus a distance of f away from the lens. Therefore, this model is essentially the same as a pinhole model if the lens is focused at a distance of infinity. Figure 6.3: The thin lens model. 6.2.3 Radial Distortion The pinhole camera model provides a nominal camera model for which it is relatively straightforward to develop a mathematical model of the perspective projection. However, in practice, this model is not a perfect representation of the imaging process. One effect that is not captured by the pinhole model is radial distortion, which is an effect seen in real lenses where either barrel distortion or camera models and calibration 175 pincushion distortion will affect the real pixel coordinates. We show images of barrel and pincushion distortion in Section 6.2.3. Figure 6.4: Different kinds of ra- dial distortions that are seen in real lenses, which may affect the accuracy of the pinhole camera model. There are methods we can use to correct for image distortion. A simple and efficient way is to model the relationship between the ideal pixel coordinates, ( u, v ) , and the distorted pixel coordinates, ( u d , v d ) , as: " u d v d # = (1 + kr 2 ) " uâ u cd vâ v cd # + " u cd v cd # ,(6.4) where k â R is the radial distortion factor, ( u cd , v cd ) are the pixel coordinates of the image center, and r 2 = (uâ u cd ) 2 + (vâ v cd ) 2 is the square of the distance between the ideal pixel location and the center of distortion. Note that k differs across cameras and needs to be predetermined through calibration. 6.3 RGB-D Cameras While traditional cameras capture color information through RGB channels, RGB- D cameras additionally provide depth information for each pixel, creating a 2.5D representation of the scene. These sensors have become increasingly important in robotics due to their ability to provide both appearance and geometric informa- tion in a single, compact package. 6.3.1 Depth Sensing Technologies RGB-D cameras employ various technologies to capture depth information along- side color: Structured light. These sensors project a known infrared pattern onto the scene and use triangulation to compute depth from the patternâs deformation. The original Microsoft Kinect exemplifies this approach, projecting a speckle pattern that is captured by an IR camera offset from the projector. Structured light sensors provide high accuracy at close range (0.5 to 4 m) but struggle in bright ambient light conditions. 176 principles of robot autonomy Time-of-flight (ToF). ToF cameras emit modulated infrared light and measure the phase shift of the reflected signal to determine distance. These sensors offer good accuracy across their operating range and work well in varying lighting condi- tions, though they typically have lower resolution than structured light sensors and can suffer from multipath interference. Stereo infrared. Some RGB-D cameras, such as the Intel RealSense D-series, use stereo vision with infrared cameras and an optional IR pattern projector. This approach combines the robustness of stereo vision with active illumination to handle textureless surfaces. 6.3.2 Applications in Robotics and Practical Considerations RGB-D cameras have enabled significant advances in several robotics applica- tions, particularly in structured indoor environments. For indoor navigation and mapping, RGB-D sensors are ideal when GPS is unavailable and lighting can be controlled. They provide dense 3D point clouds at frame rate, enabling real-time obstacle avoidance and map building, with popular SLAM systems like RGB-D SLAM and ElasticFusion leveraging these sensors for creating detailed 3D maps. In object manipulation tasks, RGB-D cameras excel by providing both the objectâs appearance and its precise 3D geometry, allowing robots to identify objects using color and texture while planning grasps based on accurate depth information. The relatively high accuracy at close range (typically 0.5 to 4 m) makes them well-suited for tabletop manipulation and pick-and-place operations. However, several practical considerations limit their use. Most consumer RGB- D sensors operate effectively only between 0.5 and 5 meters, with accuracy de- grading quadratically with distanceâmillimeter accuracy at one meter may de- grade to several centimeters at maximum range. Material properties significantly affect performance: transparent and highly reflective surfaces cannot be mea- sured, while dark materials may return weak signals leading to missing depth values. RGB-D cameras also require careful calibration between the color and depth sensors, which are typically offset from each other, and factory calibra- tion may not suffice for high-precision applications. Additionally, multiple RGB- D cameras can interfere with each otherâs IR projectors, and ambient infrared light (such as sunlight) makes outdoor use challenging. Despite these limitations, RGB-D cameras offer an attractive balance of cost, size, and capability for many indoor robotics applications, providing rich 3D perception without the computa- tional complexity of stereo matching or the cost of laser scanners. 6.4 Perspective Projection and Coordinate Transformations The pinhole camera model can be used to mathematically define relationships between points in the scene and points on the image plane. Our objective is to derive a mathematical model that maps a point P W expressed in world frame camera models and calibration 177 coordinates to a point p on the image plane in pixel coordinates. We accomplish this by combining two transformations: from world frame to camera frame coor- dinates (P W â P C ), and from camera coordinates to image coordinates (P C â p). 6.4.1 Camera Frame to Image Coordinates (P C â p) The first step we consider is how to map a point in the scene expressed in camera frame coordinates, P C , to the corresponding point on the image plane, p, using the pinhole camera model. In Chapter 5, we presented the pinhole camera equations: x = f X C Z C ,y = f Y C Z C ,(6.5) where P C = ( X C , Y C , Z C ) , p = ( x, y ) , and f is the focal length of the pinhole camera 3 . 3 We generally do not include the z term of p simply because z = f is a fixed value. Note that the quantities x and y are coordinates in the camera frame, but it is often desirable to express the point p in terms of pixel coordinates. Pixel coordi- nates are generally defined with respect to a reference frame in the lower corner of the image plane to avoid negative coordinates. We show this new reference frame in Section 6.4.1, where we define the image center, c, with coordinates ( Ì x 0 , Ì y 0 ) , where Ì (·) is the notation we use to denote a coordinate with respect to this new reference frame. In this new reference frame, we map the point P C to Figure 6.5: We define a new ref- erence frame with coordinates denoted by Ì (·) with its origin in the lower corner of the im- age plane. The image center co- ordinates in this new frame are ( Ì x 0 , Ì y 0 ). the coordinates ( Ì x, Ì y ) by: Ì x = f X C Z C + Ì x 0 , Ì y = f Y C Z C + Ì y 0 .(6.6) Finally, given the number of pixels per unit distance, we can map these new coordinates to pixel coordinates. In particular, we map the point P C to pixel coordinates ( u, v ) by: u = α X C Z C + u 0 ,v = ÎČ Y C Z C + v 0 ,(6.7) where α = k x f , u 0 = k x Ì x 0 , ÎČ = k y f , v 0 = k y Ì y 0 , and k x and k y are the number of pixels per unit distance in image coordinates. 178 principles of robot autonomy Note that the transformation from the point P C in camera frame coordinates to p in pixel coordinates given by Equation (6.7) is not linear. However, we can represent this transformation as a linear mapping 4 through an additional change 4 Expressing the perspective projection as a linear map will simplify the math- ematics later on. of coordinates. In particular, we will express the points P C and p in homogeneous coordinates. For a two-dimensional point ( x 1 , x 2 ) or a three-dimensional point ( x 1 , x 2 , x 3 ) in Euclidean space, we represent the point in homogeneous coordinates by the transformation: ( x 1 , x 2 ) â ( αx 1 , αx 2 , α ) ,and ( x 1 , x 2 , x 3 ) â ( αx 1 , αx 2 , αx 3 , α ) ,(6.8) for any α Ìž= 0. These new coordinates are called homogeneous coordinates be- cause we can choose the scaling factor, α, arbitrarily as long as α Ìž= 0. We trans- form a set of homogeneous coordinates back by: ( y 1 , y 2 , y 3 ) â y 1 y 3 , y 2 y 3 ,and ( y 1 , y 2 , y 3 , y 4 ) â y 1 y 4 , y 2 y 4 , y 3 y 4 .(6.9) We will denote when a point is described in homogeneous coordinates using the superscript h. For example, we express the point P C = ( X C , Y C , Z C ) in camera frame coordinates with α = 1 in homogeneous coordinates by: P h C = ( X C , Y C , Z C , 1 ) , and we can express the pixel coordinate p = ( u, v ) in homogeneous coordinates by: p h = ( Z C u, Z C v, Z C ) = ( αX C + u 0 Z C , ÎČY C + v 0 Z C , Z C ) , by choosing α = Z C and substituting the expressions from Equation (6.7). With the expression of these points in homogeneous coordinates, we can see that their relationship is transformed from the nonlinear relationship in Equation (6.7) to the linear relationship: ïŁź ïŁŻ ïŁ° α0u 0 0 0ÎČv 0 0 0010 ïŁč ïŁș ïŁ» ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁ° X c Y c Z c 1 ïŁč ïŁș ïŁș ïŁș ïŁ» = ïŁź ïŁŻ ïŁ° αX c + u 0 Z c ÎČY c + v 0 Z c Z c ïŁč ïŁș ïŁ» .(6.10) Often, in practice, we also add a skewness parameter, Îł 5 , and we can write 5 The skewness parameter generally ends up being close to zero. this linear relationship in the more compact form: h K0 3Ă1 i P h C = p h ,K : = ïŁź ïŁŻ ïŁ° α Îłu 0 0ÎČv 0 001 ïŁč ïŁș ïŁ» .(6.11) We refer the matrix K in Equation (6.11) as the camera matrix or matrix of intrinsic parameters because it contains the five parameters that define the fundamental characteristics of the camera from the perspective of the pinhole camera model. While these parameters may be specified by the camera manufacturer, we often estimate them in practice by performing a camera calibration. camera models and calibration 179 6.4.2 World Frame to Camera Frame (P W â P C ) We can express a point, P, in the scene (see Section 6.2.1) either in terms of camera frame coordinates, P C , or world frame coordinates, P W . While we discussed the use of the pinhole model to map P C coordinates to pixel coordinates, p, in the previous section, in this section we discuss the mapping between the camera and world frame coordinates of the point P, as we show in Section 6.4.2. Figure 6.6: A depiction of the point P expressed either in cam- era coordinates, P C , or in world frame coordinates, P W . We de- note the world frame origin by O W and the camera frame origin by O C . From Section 6.4.2, we can write P C as: P C = t + q,(6.12) where t is the vector from O C to O W , expressed in camera frame coordinates, and q is the vector from O W to P, expressed in camera frame coordinates. However, the vector q is the same vector as P W , just expressed with respect to a different coordinate frame. The coordinates are related by a rotation: q = RP W ,(6.13) where R is the rotation matrix relating the camera frame to world frame defined as: R : = ïŁź ïŁŻ ïŁ° i w · ij w · ik w · i i w · j w · jk w · j i w · kj w · k w · k ïŁč ïŁș ïŁ» ,(6.14) where i, j, and k are the unit vectors that define the camera frame and i w , j w , and k w are the unit vectors that define the world frame. To summarize, we can map the point P W to camera frame coordinates P C by: P C = t + RP W ,(6.15) where t is the vector in camera frame coordinates from O C to O W and R is the rotation matrix defined in Equation (6.14). Similar to the previous section, we can 180 principles of robot autonomy equivalently express this transformation for the case where the points P W and P C are expressed in homogeneous coordinates: " P C 1 # = " Rt 0 1Ă3 1 #" P W 1 # .(6.16) 6.4.3 Complete Projection Pipeline (P W â p) The objective of the perspective projection task is to find a way to mathematically relate the position of a point in world frame coordinates, denoted P W , to the corresponding pixel coordinates, p, on the image plane. With the relationship from Equation (6.16) that we developed for mapping P W to the camera frame coordinates, P C , and the relationship in Equation (6.11) for mapping P C to pixel coordinates, p, we can now define the direct mapping from P W to p. In particular, combining the two transformations together yields: p h = h K0 3Ă1 i " Rt 0 1Ă3 1 # P h W , which we can simplify to: p h = K h Rt i P h W .(6.17) In Equation (6.17), P h W is the homogeneous coordinate representation of P W and p h is the homogeneous coordinate representation of p. Recall that the matrix K â R 3Ă3 is the matrix of intrinsic camera parameters, and the matrix [R t] â R 3Ă4 contains extrinsic parameters 6 . Note that the total number of degrees of freedom 6 Extrinsic parameters describe the cam- eraâs position and orientation relative to the points in the scene. is 11, where 5 are from the intrinsic parameters that define K, 3 are from the rotation matrix, R, and 3 are from the position vector, t. 6.5 Camera Calibration Before we can use camera models in practice, we need to determine the cameraâs intrinsic and extrinsic parameters. Camera calibration is the process of estimating these parameters, which is essential for quantitative computer vision applications. We present two main approaches: the Direct Linear Method for basic calibration, and auto-calibration for cases where calibration targets are unavailable. 6.5.1 Direct Linear Method One approach is the direct linear calibration method, which requires a set of known correspondences, p i ââ P W,i for i = 1, . . . , n. Direct linear calibration: Step 1. For direct linear calibration, the first step is to write each corresponding pair of points, p i = ( u i , v i ) and P W,i = ( X W,i , Y W,i , Z W,i ) , in homogeneous coordinates and then use the expression in Equation (6.17) to write: p h i = MP h W,i ,i = 1, . . . , n,(6.18) camera models and calibration 181 where we refer to M = K[R t] as the projection matrix. Next, we use the n cor- respondences to estimate the projection matrix, M, and then later we can extract the intrinsic and extrinsic parameters from M. A useful first step to determine M is to rewrite it in terms of its rows: M = ïŁź ïŁŻ ïŁ° m 1 m 2 m 3 ïŁč ïŁș ïŁ» ,(6.19) where m i â R 1Ă4 is the i-th row of M. By considering the rows of M individually, we can write the relationship in Equation (6.18) as: ïŁź ïŁŻ ïŁ° αu i αv i α ïŁč ïŁș ïŁ» = ïŁź ïŁŻ ïŁ° m 1 · P h W,i m 2 · P h W,i m 3 · P h W,i ïŁč ïŁș ïŁ» ,i = 1, . . . , n which by mapping the homogeneous coordinates, p h i , back to the original coordi- nates, p i , yields the 2n expressions: u i = m 1 · P h W,i m 3 · P h W,i ,i = 1, . . . , n, v i = m 2 · P h W,i m 3 · P h W,i ,i = 1, . . . , n, or equivalently, by some algebraic manipulation, yields the expressions: u i (m 3 · P h W,i )â (m 1 · P h W,i ) = 0,i = 1, . . . , n v i (m 3 · P h W,i )â (m 2 · P h W,i ) = 0,i = 1, . . . , n. (6.20) We can now combine these 2n equations together in one large matrix equation: Ì Pm = 0,m : = ïŁź ïŁŻ ïŁ° m †1 m †2 m †3 ïŁč ïŁș ïŁ» ,(6.21) where mâ R 12Ă1 is a vector consisting of the stacked rows of M and Ì Pâ R 2nĂ12 is a matrix of known coefficients determined by the quantities u i , v i , and P h W,i . For a more concrete representation of how we define Ì P, the first couple rows are given by: Ì P = ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° â(P h W,1 ) †0 1Ă4 u 1 (P h W,1 ) †0 1Ă4 â(P h W,1 ) †v 1 (P h W,1 ) †â(P h W,2 ) †0 1Ă4 u 2 (P h W,2 ) †. . . . . . . . . ïŁč ïŁș ïŁș ïŁș ïŁș ïŁ» .(6.22) Note that we must have at least six correspondences, n â„ 6, to ensure that m is uniquely defined. With this sufficient number of correspondences, we could ideally directly solve Equation (6.21). However, in practice, a more robust proce- dure is to build Ì P with more than 6 points, which gives an overdetermined set of 182 principles of robot autonomy equations that may not have a solution 7 . Therefore, to compute m, we formulate 7 This is particularly true in real-world applications where noise corrupts the data. an optimization problem: minimize m â„ Ì Pmâ„ 2 , subject to â„mâ„ 2 = 1, (6.23) where the constraint â„mâ„ 2 = 1 is required to ensure that the optimization prob- lem cannot be solved by trivially choosing m i = 0 for each i = 1, . . . , 12. We call this optimization problem a constrained least-squares problem. Example 6.5.1 (Constrained least-squares optimization). The constrained least squares problem: minimize x â„ Axâ„ 2 , subject to â„xâ„ 2 = 1, with x â R n and A â R mĂn and m > n is a finite-dimensional optimization problem. Consider the corresponding Lagrangian: L = x †A †Ax + λ(1â x †x), and the necessary optimality conditions: â x L = 2(A †Aâ λ I)x = 0, â λ L = 1â x †x = 0. We can write the first necessary optimality condition as A †Ax = λx, and there- fore any x that satisfies this condition must be an eigenvector of the matrix A †A. Additionally, while all the eigenvectors satisfy this condition, the optimum is the eigenvector associated with the smallest eigenvalue. We can efficiently compute this eigenvector by using a singular value decomposition of A = UÎŁV †and then choosing x to be the column of V associated with the smallest singular value, since A †A = VÎŁ 2 V †. Direct linear calibration: Step 2. Once we have solved the optimization problem in Equation (6.23) to compute the vector m, the projection matrix, M, is completely defined. The next step in the camera calibration process is to extract the intrinsic and extrinsic camera parameters from the matrix M. For this step, we will express the matrix M in terms of its columns: M = h c 1 c 2 c 3 c 4 i , where c i is the i-th column of M. We can factorize M as: M = K h Rt i ,(6.24) by taking the first three columns of M and performing a RQ factorization: h c 1 c 2 c 3 i = KR,(6.25) where R is an orthogonal matrix and K is an upper triangular matrix. Once K is known, we can compute the vector t by t = K â1 c 4 . camera models and calibration 183 6.5.2 A Flexible Camera Calibration Method The projection matrix, M, is defined for a specific set of extrinsic parameters R and t. In practice, however, it might be desirable for us to estimate the cam- eraâs intrinsic parameters from N different images from different perspectives, and therefore with N different projection matrices due to the varying extrinsic parameters. In this case, we can apply an alternative procedure 8 to the direct 8 Z. Zhang. âA Flexible New Technique for Camera Calibrationâ. In: IEEE Trans- actions on Pattern Analysis and Machine In- telligence 22 (2000) linear calibration method to extract the intrinsic parameters, K. We begin by assuming that the known points, P W , for each individual image lie on a plane. For example, the calibration scene might consist of a pattern, such as a checkerboard pattern, on a planar surface. In this case, we can assume that the world frame origin lies on the plane such that Z W = 0 for all points on the plane. Since Z W = 0, we can simplify the relationship between p h and P h W given by Equation (6.17) to: p h = H Ì P h W ,(6.26) with: H = K h r 1 r 2 t i , Ì P h W = h X W Y W 1 i †,(6.27) where H is called the homography matrix 9 , Ì P h W is the simplified position of the 9 The homography matrix maps points betweentwo-dimensionalplanes while a projection matrix maps three- dimensional points to points on a two-dimensional plane. point P in world frame, written in homogeneous coordinates, and r i is the i-th column of the rotation matrix, R. Note that we can still estimate the homography matrix, H, using the same procedure discussed earlier. Next, we identify a set of constraints on the intrinsic parameter matrix, K, by writing the homography, H, as: H = h Kr 1 Kr 2 Kt i = h Ì c 1 Ì c 2 Ì c 3 i , and noting that since r 1 and r 2 are orthonormal we have: Ì c †1 B Ì c 2 = 0, Ì c †1 B Ì c 1 = Ì c †2 B Ì c 2 ,(6.28) where B = K â†K â1 â R 3Ă3 is a symmetric matrix. We can therefore solve for the intrinsic camera parameters, K, by using the constraints in Equation (6.28) to solve for the symmetric matrix B and then backing out the parameters that define K. To compute the matrix B from the constraints in Equation (6.28), we can employ several useful tricks. The main trick is to notice that even though B consists of nine parameters, it is symmetric, and, therefore, we only need six parameters to specify it fully. Therefore, we reparameterize the matrix B â R 3Ă3 as a vector bâ R 6 as: b = h B 11 B 12 B 22 B 13 B 23 B 33 i †.(6.29) This reparameterization is useful because it allows us to rewrite the expression Ì c †i B Ì c j as: Ì c †i B Ì c j = v †ij b,(6.30) 184 principles of robot autonomy where: v ij = h Ì c i1 Ì c j1 , Ì c i1 Ì c j2 + Ì c i2 Ì c j1 , Ì c i2 Ì c j2 , Ì c i3 Ì c j1 + Ì c i1 Ì c j3 , Ì c i3 Ì c j2 + Ì c i2 Ì c j3 , Ì c i3 Ì c j3 i †, and where Ì c ik is the k-th element of the column vector Ì c i and Ì c jk is the k-th ele- ment of the column vector Ì c j . With this reparameterization, we can rewrite the constraints in Equation (6.28) as: Ì c †1 B Ì c 2 = 0 =â v †12 b = 0, Ì c †1 B Ì c 1 = Ì c †2 B Ì c 2 =â (v 11 â v 22 ) †b = 0, or by combining them: " v †12 (v 11 â v 22 ) †# b = 0,(6.31) which is linear with respect to the unknowns vector b. Importantly, while the homographies, H, are different for each image due to the different extrinsic pa- rameters, the intrinsic camera parameters represented by the vector b are the same. Therefore, with N images from the same camera, even with potentially different perspectives, we can stack the constraints in Equation (6.31) to give: Vb = 0,(6.32) where V â R 2NĂ6 . In the case where we include the skewness parameter, Îł, in K, there must be N â„ 3 images in order to specify B uniquely. Similarly to the approach for computing the projection matrix in the previous section, we can compute the vector b as the solution to the constrained least squares problem: minimize b â„Vbâ„ 2 , subject to â„bâ„ 2 = 1. (6.33) Once we have computed b, we can solve for the intrinsic camera parameters, K, by leveraging the definition of B = K âT K â1 . In particular, we compute the intrinsic parameters by: v 0 = B 12 B 13 â B 11 B 23 B 11 B 22 â B 2 12 , λ = B 33 â B 2 13 + v 0 (B 12 B 13 â B 11 B 23 ) B 11 , α = s λ B 11 , ÎČ = s λB 11 B 11 B 22 â B 2 12 , Îł = âB 12 α 2 ÎČ Î» , u 0 = Îłv 0 ÎČ â B 13 α 2 λ , (6.34) camera models and calibration 185 where we can think of λ as a scaling parameter that accounts for the fact that there are five unknown camera intrinsic parameters but six degrees of freedom in B. Once we have extracted the camera intrinsic parameters, K, from this proce- dure, given any new homography, H, we can compute the extrinsic parameters by: r 1 = K â1 Ì c 1 â„K â1 Ì c 1 â„ , r 2 = K â1 Ì c 2 â„K â1 Ì c 2 â„ , r 3 = r 1 Ă r 2 , t = K â1 Ì c 3 â„K â1 Ì c 1 â„ . (6.35) As one final step, we note that the matrix R defined with column vectors r 1 , r 2 , and r 3 will not generally satisfy the orthonormality property of a rotation matrix. We can correct this issue by again using optimization methods to compute a valid rotation matrix that best corresponds to these column vectors: minimize R â„Râ Qâ„ 2 , subject toR †R = I, (6.36) where: Q = h r 1 r 2 r 3 i . We solve this problem by choosing R = UV †, where U and V are defined by the singular value decomposition of Q = UÎŁV †. 6.5.3 Camera Auto-Calibration The direct linear transformation from Section 6.5.1 and Zhangâs flexible calibra- tion method from Section 6.5.2 require point correspondences to calculate the in- trinsic and extrinsic parameters. Auto-calibration offers an alternative approach that does not make this assumption by utilizing multiple views of a static scene to determine the cameraâs intrinsic and extrinsic parameters. This approach lever- ages the fact that a cameraâs intrinsic parameters remain constant across different views of the same scene. By identifying correspondences between points in mul- tiple images, we can estimate the intrinsic matrix, K, and extrinsic matrices, R and t, by bundle adjustment based on the static scene geometry. In total, the camera auto-calibration process consists of five key steps. First, we use an algorithm such as scale-invariant feature transform (SIFT) to identify key points in the scene and their correspondence points across multiple images from different views. The SIFT algorithm is a computer vision method to detect, describe, and match local features in images. It is robust in detecting 186 principles of robot autonomy and describing local features in images, making it effective for finding correspon- dences under varying conditions. Details of the SIFT algorithm will be discussed in the following chapters. Second, we use the correspondences between a pair of images to compute the fundamental matrix. The fundamental matrix, F â R 3Ă3 , relates corresponding points between a pair of images of the same scene 10 . For a pair of images (I, I âČ ) 10 The fundamental matrix describes the epipolar geometry between two image views. of the same scene, the fundamental matrix, F, satisfies: p †F p âČ = 0,(6.37) where p and p âČ are corresponding points in images I and I âČ , respectively. We will discuss the fundamental matrix more in the context of stereo vision in Chapter 7. To compute the fundamental matrix for the image pair, we first construct a point correspondence matrix, W, where each row represents a correspondence between points in the two images. Let (u i , v i ) and (u âČ i , v âČ i ) be the coordinates of one set of matched points, p i and p âČ i . We form each row of the matrix W using one set of corresponding coordinates as: W = ïŁź ïŁŻ ïŁŻ ïŁ° u 1 u âČ 1 u 1 v âČ 1 u 1 v 1 u âČ 1 v 1 v âČ 1 v 1 u âČ 1 v âČ 1 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . u n u âČ n u n v âČ n u n v n u âČ n v n v âČ n v n u âČ n v âČ n 1 ïŁč ïŁș ïŁș ïŁ» .(6.38) From this point correspondence matrix, we then compute the fundamental ma- trix by solving the linear system W f = 0 using a singular value decomposition (SVD), where f is the vectorized form of the fundamental matrix. We describe the specifics of this procedure in more detail in Section 7.1.1. Third, we compute the essential matrix, which is similar to the fundamental ma- trix in that it relates corresponding points in two images based on scene geometry. For two images, I and I âČ , we define the essential matrix by the rotation matrix, R, and translation vector, t, relating the coordinate frames of the two images by: E = [t] Ă R,(6.39) where [t] Ă is the matrix representation of the cross product. Assuming the in- trinsic matrix, K, remains the same between the images, we can also define the essential matrix, E, with respect to the fundamental matrix as: E = K †FK.(6.40) Therefore, we can first compute the essential matrix from the previously com- puted fundamental matrix and the intrinsic parameter matrix, K, and then we can compute the camera extrinsic rotation and translation parameters, R and t, by the singular value decomposition: E = UÎŁV †,(6.41) camera models and calibration 187 whereÎŁ = diag(1, 1, 0). From this decomposition, there are two possible solu- tions for the camera extrinsic parameters: R 1 = UWV †,R 2 = UW †V †, t 1 = U[:, 2],t 2 =âU[:, 2], (6.42) where: W = ïŁź ïŁŻ ïŁ° 0 â1 0 100 001 ïŁč ïŁș ïŁ» .(6.43) We can identify the correct solution by using each to compute the three-dimensional points from the two-dimensional matched correspondence points, as we describe in the next step. The extrinsic parameters, R and t, we should use are the ones that ensure that most of the three-dimensional points lie in front of both cameras, meaning they will have positive depth values. The fourth step is to perform triangulation to compute the 3D points in the scene from the 2D image correspondence points. We define the projection matrix, P, for an image as: P = K h Rt i ,(6.44) where again K is the camera intrinsic matrix, R is the rotation matrix, and t is the translation vector. This projection matrix maps a 3D point in homogeneous coordinates into the 2D camera frame coordinates by: ïŁź ïŁŻ ïŁ° u v 1 ïŁč ïŁș ïŁ» = P ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁ° X Y Z 1 ïŁč ïŁș ïŁș ïŁș ïŁ» .(6.45) Therefore, for matched points across two images, we can compute the 3D coordi- nates by solving the linear system: ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁ° uP †3 â P †1 vP †3 â P †2 u âČ P âȆ3 â P âȆ1 v âČ P âȆ3 â P âȆ2 ïŁč ïŁș ïŁș ïŁș ïŁ» ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁ° X Y Z 1 ïŁč ïŁș ïŁș ïŁș ïŁ» = 0,(6.46) where (u, v) are the coordinates and P 1 , P 2 , and P 3 are the first, second, and third rows of the projection matrix, P, for image I, respectively, and (u âČ , v âČ ) are the coordinates and P âČ 1 , P âČ 2 , and P âČ 3 are the rows of the projection P âČ for image I âČ . We can solve Equation (6.46) using a least squares method. So far, we have used correspondences in pairs of images from the same cam- era of the same scene to estimate the image extrinsics, R and t, and computed estimates of the 3D scene points by triangulation. These computations require knowledge of the camera intrinsic matrix, K, which is the quantity we are try- ing to estimate. We can leverage the previous steps to compute K by using an iterative optimization-based procedure, where we begin with an estimate 11 of K 11 For example, we could start with an es- timate by referencing the camera manu- facturerâs data, or using some other sim- pler method. 188 principles of robot autonomy and refine it until convergence. In particular, we refine the estimate of the camera intrinsics and extrinsics by solving the optimization: N â i=1 M â j=1 â„ p ij â P i (K, R i , t i )X j â„ 2 ,(6.47) where p ij is an observed 2D point in image i that corresponds to the j-th 3D point, X j is the j-th estimated 3D point, P i (K, R i , t i ) is the projection matrix for image i, M is the total number of triangulated 3D points, and N is the total number of images. Note that each projection matrix is a function of the intrinsic parame- ters, which are constant across all images, as well as the extrinsic parameters for the image. We can optimize this cost function by applying a nonlinear method, such as Levenberg-Marquardt, to compute a new set of parameters. We then re- peat the steps listed above, computing new extrinsics, triangulation points, and optimizing, until convergence. 6.6 RGB-D Camera Calibration RGB-D cameras present additional calibration challenges beyond traditional cam- eras. In addition to calibrating the RGB camera using standard methods, we must also: âą Calibrate the depth sensorâs intrinsic parameters âą Determine the extrinsic transformation between RGB and depth sensors âą Correct for systematic depth measurement errors âą Account for the different fields of view and resolutions of the two sensors Many manufacturers provide factory calibration, but applications requiring high accuracy often necessitate custom calibration procedures using specialized targets that are visible in both RGB and infrared. 6.7 Summary This chapter presented the mathematical foundations of camera models and cal- ibration techniques essential for vision-based robotics. Starting from the basic pinhole model, we developed the perspective projection equations that relate 3D world points to 2D image coordinates. We explored practical considerations including lens models and distortion, and extended our discussion to modern RGB-D sensors that provide both color and depth information. The calibration methods presented enable us to estimate the camera parameters necessary for quantitative vision applications. With calibrated cameras, we can now proceed to extract three-dimensional information from images, which we explore in the next chapter through stereo vision and structure from motion techniques. camera models and calibration 189 To learn more. For a deeper dive into camera models and calibration techniques, readers are encouraged to refer to the Foundations of Computer Vision textbook by Torralba, Isola, and Freeman [150] for a more in-depth discussion on image systems and camera models. For practical calibration algorithms and implemen- tations, the OpenCV library 12 provides extensive resources and code examples. 12 G. Bradski. âThe OpenCV Libraryâ. In: Dr. Dobbâs Journal of Software Tools (2000) Interested readers can consult the works directly from Tsai [152], Lowe [87], and Triggs et al. [151] for further insights into the algorithms discussed. 6.8 Exercises The starter code for the exercises provided below is available online through GitHub. To get started, download the code by running in a terminal window: git clone https://github.com/StanfordASL/pora-exercises.git We denote Problems requiring hand-written solutions and coding in Python with and, respectively. Problem 1: Camera Calibration: Extrinsics In this exercise, you will implement the key parts of a method to compute the camera extrinsic parameters R and t. Specifically, you will use the steps in Sec- tion 6.5.1 to compute the homography matrix H given a calibration image. Then, given the camera intrinsic matrix K, you will use the method at the end of Sec- tion 6.5.2 to compute the extrinsics, R and t, for the image. For this exercise, you will use a chessboard with known dimensions to help with the camera calibration process. This is a convenient choice because it provides a high contrast and the grid corner features are easy to detect. In the file ch06/exercises/camera _ extrinsics.ipynb: 1. First, implement the function generate _ chessboard _ 3D _ world _ coordinates to compute a grid of 3D world coordinates that correspond to the grid corners of the chessboard. Note that we can choose to set all of the z-coordinates to be 0 since we know the chessboard is a 2D plane. Then, implement the function generate _ chessboard _ 2D _ pixel _ coordinates to compute the 2D pixel coordinates of the grid corners from an image using the function findCh essboardCorners from the open-source computer vision package cv2. 2. Second, implement the function compute _ homography using the method from Section 6.5.1. Note that generally in Section 6.5.1 we are computing the pro- jection matrix M â R 3Ă4 , but in this problem we have a slightly simplified problem of computing the homography matrix H â R 3Ă3 which just maps points between two 2D planes (since we know the chessboard grid points lie on a common plane). In other words, when using the approach from Sec- tion 6.5.1 rather than dealing with rows m i â R 1Ă4 you should be consid- 190 principles of robot autonomy ering rows of the homography matrix h i â R 1Ă3 and be using the vectors P h W = [X W , Y W , 1] †. Once the homography matrix is available, implement the function compute _ extrinsics using the method at the end of Section 6.5.2 to compute the extrinsics R and t given the intrinsic matrix K. Additionally, implement the function transform _ world _ to _ pixel to transform a point in the world frame into the pixel coordinates from the camera intrinsics and ex- trinsics. Run the provided code to see the result of your computations on some example chessboard images. Problem 2: Camera Calibration: Intrinsics In Problem 1, we provided the camera intrinsic matrix, K. In this exercise 13 , you 13 Note you will need to first complete Problem 1 since this will leverage some of that problemâs code. will use the flexible calibration method described in Section 6.5.2 to compute the intrinsic matrix yourself. Specifically, explore the notebook ch06/exercises/camera _ intrinsics.ipynb. You will need to implement the function compute _ intrinsics to compute the matrix K from a list of homography matrices from different calibration images. camera models and calibration 191 References [20]G. Bradski. âThe OpenCV Libraryâ. In: Dr. Dobbâs Journal of Software Tools (2000). [87]D. G. Lowe. âObject recognition from local scale-invariant featuresâ. In: Proceedings of the seventh IEEE international conference on computer vision. Vol. 2. Ieee. 1999, p. 1150â1157. [150]A. Torralba, P. Isola, and W. T. Freeman. Foundations of Computer Vision. Available under C-BY-ND-NC license. Cambridge, MA: The MIT Press, 2024. isbn: 978-0-262-04897-2. [151]B. Triggs et al. âBundle adjustmentâa modern synthesisâ. In: Vision Al- gorithms: Theory and Practice: International Workshop on Vision Algorithms Corfu, Greece, September 21â22, 1999 Proceedings. Springer. 2000, p. 298â 372. [152]R. Tsai. âA Versatile Camera Calibration Technique for High-accuracy 3D Machine Vision Metrology Using Off-the-shelf TV Cameras and Lensesâ. In: IEEE Journal on Robotics and Automation 3.4 (1987), p. 323â344. [165]Z. Zhang. âA Flexible New Technique for Camera Calibrationâ. In: IEEE Transactions on Pattern Analysis and Machine Intelligence 22 (2000). 7 Stereo Vision and Structure From Motion In Chapter 6, we introduced the mathematical relationship between the position of a point, P, in a scene, expressed in world frame coordinates, P W , and the corre- sponding point, p, in pixel coordinates that gets projected onto the image plane of a camera. This relationship is based on the pinhole camera model, and requires knowledge about the cameraâs intrinsic and extrinsic parameters. We also pre- sented methods for camera calibration, which allow us to determine these param- eters. Given a calibrated camera with known parameters, a fundamental problem in robotic perception is how to leverage images to recover three-dimensional in- formation about the structure of the environment 1 . The camera projection model 1 While we could use other sensors to re- cover three-dimensional scene informa- tion, such as ultrasonic sensors or laser rangefinders, cameras capture a broad range of information that goes beyond depth sensing and are attractive based on their cost and size. alone does not provide us with enough information to fully determine the 3D position of a point from a single image, specifically because we cannot determine the pointâs depth 2 . 2 Unless you are willing to make strong assumptions, for example that you know the physical dimensions of the objects in the environment. In this chapter, we introduce stereo vision in Section 7.1 and structure from mo- tion in Section 7.3, two approaches for extracting 3D information from camera images. Both leverage multiple images of a scene to determine three-dimensional structure: stereo vision uses images from different viewpoints captured simul- taneously by two or more cameras, while structure from motion uses images captured sequentially from a single moving camera. These techniques form the visual foundation for many robotic applications including navigation, mapping, and manipulation. 7.1 Stereo Vision Stereopsis 3 is the process in visual perception leading to the sensation of depth 3 From stereo, meaning solidity, and opsis, meaning vision or sight. from two slightly different projections of the world onto the retinas of the two eyes. The difference in the two retinal images is called horizontal disparity, retinal disparity, or binocular disparity, and arises from our eyesâ different positions in the head. This disparity enables our brain to fuse the two retinal images into a single percept with depth information. For example, if you hold your finger vertically in front of you and alternate closing each eye, you will see that the finger jumps from left to rightâthis lateral displacement is the disparity between your eyes. This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 194 principles of robot autonomy Computational stereopsis, or stereo vision, is the process of obtaining depth information from images captured by two or more cameras observing the same scene from different perspectives. This process consists of two major steps: fusion and reconstruction. Fusion involves solving the correspondence problemâidentifying which pixels in each image correspond to the same 3D point. Reconstruction uses these correspondences to triangulate the 3D position of scene points, including their depth. 7.1.1 Epipolar Geometry The first step in stereo vision is to establish correspondences between images 4 . 4 We generally assume that the perspec- tives differ only slightly, such that fea- tures appear similar across images. This task can be challenging, as incorrect matches lead to large reconstruction errors. Epipolar geometry provides powerful constraints that simplify correspon- dence search and improve matching accuracy. Figure 7.1: The point P in the scene, the optical centers O and O âČ of the two cameras, and the two image projections p and p âČ all lie in the same epipolar plane.The lines l and l âČ are the epipolar lines.If point p is observed in one image, its correspondence must lie on the epipolar line l âČ in the other im- age. Epipolar constraints. Consider the image projections p and p âČ of a scene point P observed by two cameras with optical centers O and O âČ , as shown in Section 7.1.1. These five points all lie in the epipolar plane, defined by the two rays OP and O âČ P. The intersection of this plane with each image plane forms the epipolar lines l and l âČ , which pass through the epipoles e and e âČ âthe projections of each camera center onto the other cameraâs image plane. This geometric relationship provides a powerful constraint: if p and p âČ are projections of the same point P, then p must lie on epipolar line l and p âČ must lie on epipolar line l âČ . This epipolar constraint reduces the correspondence search from a two-dimensional problem to a one-dimensional search along epipolar lines. Mathematically, we express this constraint using the coplanarity of the vectors: O p· [O âČ ĂO âČ p âČ ] = 0.(7.1) Fundamental and essential matrices. When the world reference frame coincides with the first cameraâs frame (origin at O), we can express the epipolar constraint as: p †F p âČ = 0,(7.2) stereo vision and structure from motion 195 where F â R 3Ă3 is the fundamental matrix. The fundamental matrix encodes the epipolar geometry between two views and has seven degrees of freedom (it is defined up to scale and has rank 2). It depends only on the camerasâ intrinsic parameters and their relative pose: F = K â†EK âČâ1 ,(7.3) where K and K âČ are the intrinsic parameter matrices for the two cameras, and E is the essential matrix: E = [t] Ă R = ïŁź ïŁŻ ïŁ° 0 ât 3 t 2 t 3 0 ât 1 ât 2 t 1 0 ïŁč ïŁș ïŁ» R,(7.4) with R and t = [ t 1 , t 2 , t 3 ] †being the rotation and translation that transform points from the second camera frame to the first. The fundamental matrix also defines the epipolar lines: l = F p âČ and l âČ = F †p. The epipoles satisfy F †e = 0 and Fe âČ = 0, confirming that F is singular (rank 2). To estimate F from image correspondences, we use the fact that each cor- respondence ( p i , p âČ i ) provides one linear constraint. With p = [ u, v, 1 ] †and p âČ = [ u âČ , v âČ , 1 ] †in homogeneous coordinates, we can rewrite the epipolar con- straint as: h u âČ uv âČ uvu âČ v âČ vu âČ v âČ 1 i f = 0,(7.5) where f is the vectorized form of F. Given nâ„ 8 correspondences, we stack these constraints into a matrix equation W f = 0 and solve: minimize f â„W fâ„ 2 , subject to â„ fâ„ 2 = 1. (7.6) The solution is the eigenvector corresponding to the smallest eigenvalue of W †W. Since the resulting matrix may not have rank 2, we enforce the singularity con- straint by computing the SVD of Ì F and setting the smallest singular value to zero. Image rectification. Epipolar rectification transforms stereo image pairs such that epipolar lines become horizontal and aligned across images. This transforma- tion simplifies correspondence search to a one-dimensional problem along im- age rows, significantly reducing computational cost. The rectified configuration is equivalent to having two cameras with parallel optical axes and aligned im- age rows, separated by a baseline distance. After rectification, as shown in Sec- tion 7.1.1, correspondence search is constrained to horizontal scan lines, making stereo matching algorithms more efficient and robust. 7.2 Correspondence and Reconstruction With the geometric constraints established through epipolar geometry and sim- plified through image rectification, we now turn to the practical challenges of 196 principles of robot autonomy Figure 7.2: Epipolar rectification transforms images so that corre- sponding points lie on the same horizontal scan line.Original epipolar lines (left) become hor- izontal and aligned after rectifi- cation (right). stereo vision. The correspondence problemâdetermining which pixels in each image represent the same scene pointâremains the most critical and challenging step, as errors here propagate directly to the reconstructed 3D structure. Once re- liable correspondences are found, triangulation transforms these matched points into 3D coordinates, with the accuracy depending fundamentally on the system geometry and image measurements. The correspondence problem. Even with epipolar constraints and rectification, find- ing correct correspondences remains challenging. Occlusions occur when points visible in one view are hidden in another, while repetitive patterns in textured re- gions create ambiguous matches where multiple locations appear identical. Con- versely, uniform regions lack sufficient texture for reliable feature matching, and perspective distortions cause features to appear different across viewpoints de- spite representing the same scene point. Modern stereo matching algorithms address these challenges through robust feature descriptors, correlation-based matching over local windows, or learned representations that capture scene se- mantics beyond low-level appearance. Triangulation and disparity. Once correspondences are established, we reconstruct 3D points through triangulation. For rectified stereo pairs with parallel optical axes separated by baseline b, the geometry simplifies considerably, as shown in Section 7.2. From similar triangles, we derive the fundamental stereo equation: z = b f d ,(7.7) stereo vision and structure from motion 197 Figure 7.3: Triangulation with rectified stereo cameras.The depth z is inversely proportional to the disparity d = p u â p âČ u . where z is depth, b is baseline, f is focal length, and d : = p u â p âČ u is the dispar- ityâthe difference in horizontal coordinates between corresponding points. This inverse relationship between depth and disparity has important implica- tions for stereo system design. Near objects produce large disparities and thus accurate depth estimates, while distant objects yield small disparities with corre- spondingly less accurate depth measurements. Since depth resolution decreases quadratically with distance, baseline selection becomes critical: larger baselines improve depth accuracy for distant objects but increase occlusions where one camera cannot see points visible to the other. This fundamental trade-off must be considered when designing stereo systems for specific applications. Disparity maps. A disparity map encodes the disparity value for each pixel, pro- viding a dense depth representation of the scene. Section 7.2 shows an example where brighter regions indicate larger disparities (closer objects) and black re- gions represent occluded areas where no correspondence exists. Dense disparity estimation extends the correspondence problem from sparse feature matching to every pixel, requiring additional regularization to handle ambiguous regions while preserving depth discontinuities at object boundaries. Figure 7.4: Disparity map from stereo images. Brighter values indicate larger disparities (closer objects). Black regions are oc- clusions where correspondence cannot be established. 7.3 Structure From Motion The structure from motion (SFM) method uses a similar principle as stereo vision, but uses a single camera to capture multiple images from different perspectives while moving within the scene. In this case, the intrinsic camera parameter ma- trix, K, will be constant across images, but the extrinsic parameters consisting of 198 principles of robot autonomy the rotation matrix, R, and relative position vector, t, will be different for each image. Consider a case where we take m images of n fixed 3D points from dif- Figure 7.5: A depiction of the structure from motion (SFM) method.A single camera is used to take multiple im- ages from different perspectives, which provides enough infor- mation to reconstruct the 3D scene. ferent perspectives. This would lead to m projection matrices,P k = K[R k t k ], and n 3D points, P j , that we would need to determine by leveraging the projection relationships: p h j,k =P k P h j ,j = 1, . . . , n,k = 1, . . . , m. Notice that there is quite a bit of similarity between this problem and the cam- era auto-calibration problem discussed in Chapter 6, except here we assume we already know the camera intrinsic parameters. The fundamental challenge in structure from motion is to simultaneously re- cover both the camera poses and the 3D point positions from only the 2D image observations. This requires solving a large optimization problem with many un- knowns, which we approach either incrementally by adding one image at a time, or globally by considering all images simultaneously. Both approaches rely on establishing feature correspondences across images and minimizing reprojection errors through bundle adjustment. Structure from motion methods also have some unique limitations. Most notably, there exists an inherent ambiguity in the absolute scale of the scene that cannot be determined from images alone 5 . Ad- 5 For example, a bigger object at a longer distance and a smaller object at a closer distance can yield identical projections. ditionally, errors accumulate over long sequences, leading to drift that must be corrected through loop closure when the camera revisits a previous location. De- spite these challenges, structure from motion has proven invaluable for robotic applications where carrying multiple cameras is impractical. 7.3.1 Two-View Geometry The structure from motion pipeline begins with reconstructing the scene from two views, establishing the initial geometry that subsequent images will extend. Given correspondences between two images taken from positions O 1 and O 2 , we first estimate the fundamental matrix using the same techniques described for stereo vision. With known intrinsics, we recover the essential matrix E = K †FK and decompose it to obtain the relative camera pose. stereo vision and structure from motion 199 The decomposition of the essential matrix yields four possible solutions for the rotation and translation (R, t). We identify the correct solution by verifying that reconstructed points have positive depth in both camera views. Once the relative pose is established, we triangulate the matched features to obtain initial 3D points. For each correspondence ( p j,1 , p j,2 ), the 3D point P j satisfies: " ( p j,1 Ă)P 1 ( p j,2 Ă)P 2 # P j = 0,(7.8) whereP 1 andP 2 are the projection matrices for cameras at O 1 and O 2 , and ( pĂ) denotes the skew-symmetric matrix formed from the homogeneous coordinates of p. This linear system is solved using least squares, typically with singular value decomposition for numerical stability. 7.3.2 Sequential Reconstruction Sequential structure from motion extends the initial two-view reconstruction by incrementally incorporating new images, as illustrated in Section 7.3. Each new image captured from position O k undergoes pose estimation through perspective- n-point (PnP) solving, which finds the camera position (R k , t k ) that minimizes reprojection error for correspondences between image points p j,k and existing 3D points P j . Once registered, the image contributes new scene points through triangulation with previously registered views from positions O kâ1 , O kâ2 , . . .. The critical challenge is maintaining consistency as errors accumulate. Bun- dle adjustment jointly optimizes camera poses and 3D points to minimize total reprojection error: minimize R k ,t k ,P j â k â jâV k â„ p j,k â Ï(K, R k , t k , P j )â„ 2 ,(7.9) where p j,k represents the observed projection of point P j in the image taken from camera position O k , and V k denotes the set of points visible from viewpoint k. However, global optimization after each image is computationally prohibitive. Instead, we perform local bundle adjustment over a sliding window of recent im- ages (typically spanning positions O kâw through O k ), maintaining local accuracy while deferring global consistency to a final optimization step. This windowed approach achieves near-global accuracy at a fraction of the computational cost, making the method practical for long image sequences. Image selection order also affects reconstruction qualityâwe typically choose the next viewpoint that maximizes correspondences with the current reconstruction, balancing accurate pose estimation with effective triangulation of new points. Global structure from motion. An alternative to sequential reconstruction is global structure from motion, which estimates all camera poses simultaneously before triangulating points. This approach first constructs an epipolar graph connecting all image pairs with sufficient matches, then solves for all rotations and transla- tions globally through rotation and translation averaging. While potentially more 200 principles of robot autonomy accurate than incremental methods, global approaches require solving large op- timization problems and may be less robust to outliers in practice. The choice between sequential and global methods often depends on the specific application requirements and computational resources available. 7.3.3 Visual Odometry One particularly important application of the structure from motion concept is visual odometry, which estimates robot motion in real-time using visual input. Vi- sual odometry prioritizes speed and local accuracy over global consistency, main- taining only a sliding window of recent frames and performing limited bundle adjustment. This approach operates at camera frame rates by trading global op- timality for computational efficiency. Visual odometry has proven invaluable for robot navigation, particularly in environments where wheel odometry is unreli- able or unavailable. Mars rovers, for instance, rely on visual odometry to navigate terrain where wheel slip would cause traditional odometry to fail dramatically 6 . 6 The Mars Exploration Rovers Spirit and Opportunity used stereo visual odome- try to traverse over 45 kilometers com- bined, far exceeding their planned 600- meter missions. The technique also enables navigation for flying robots and underwater vehicles where wheel odometry is impossible. Modern visual odometry systems often combine multiple approaches for ro- bustness. Feature-based methods track distinctive image features across frames, providing robustness to illumination changes but potentially failing in texture- less environments. Direct methods minimize photometric error using raw pixel intensities, exploiting all image information but requiring good initialization and small inter-frame motions. Hybrid approaches leverage the strengths of both, using features for robustness and direct alignment for accuracy. 7.3.4 Loop Closure and Drift Mitigation Over extended sequences, small errors in structure from motion accumulate into significant drift. Loop closure detection identifies when the camera revisits a pre- vious location, providing constraints to correct this accumulated error globally. This involves recognizing previously seen places despite changes in viewpoint and lighting, typically using visual vocabularies or learned features, followed by geometric verification and global optimization incorporating the loop constraints. Successfully detecting and closing loops transforms structure from motion from a local reconstruction technique into a method capable of mapping large environ- mentsâa capability essential for autonomous navigation. 7.4 Summary This chapter presented two fundamental approaches for extracting 3D informa- tion from camera images. Stereo vision leverages simultaneous views from mul- tiple cameras, using epipolar geometry to constrain correspondence search and enable real-time depth estimation through triangulation. Structure from motion stereo vision and structure from motion 201 reconstructs scene geometry from sequential images captured by a moving cam- era, trading real-time performance for the flexibility of a single-camera system. Both techniques face similar challengesâestablishing correspondences between images despite occlusions, repetitive textures, and perspective distortions. The reconstruction accuracy depends fundamentally on baseline configuration: stereo systems use fixed baselines while structure from motion adapts its effective base- line through camera motion. However, structure from motion suffers from scale ambiguity that must be resolved through additional sensors or known scene di- mensions. These complementary approaches have enabled numerous robotic ap- plications, from Mars rover navigation using visual odometry to autonomous ve- hicle perception using stereo depth. Modern systems increasingly combine both techniques with other sensorsâvisual-inertial odometry fuses structure from mo- tion with IMU data to recover metric scale, while stereo visual odometry lever- ages multiple cameras with motion estimation for robust navigation. To learn more. For a comprehensive treatment of stereo vision and structure from motion, including advanced algorithms and practical implementations, readers are encouraged to consult the textbooks Introduction to Autonomous Mobile Robots by Siegwart, Nourbakhsh, and Scaramuzza [135] and Computer Vision: A Modern Approach by Forsyth and Ponce [37]. Additionally, for a deeper understanding of epipolar geometry and multi-view reconstruction, readers can refer to the works Fusiello, Trucco, and Verri [38] and Loop and Zhang [85]. 202 principles of robot autonomy References [37]D. A. Forsyth and J. Ponce. Computer Vision: A Modern Approach. Prentice Hall, 2011. [38]A. Fusiello, E. Trucco, and A. Verri. âA compact algorithm for rectification of stereo pairsâ. In: Machine Vision and Applications 12.1 (2000), p. 16â22. [85]C. Loop and Z. Zhang. âComputing rectifying homographies for stereo visionâ. In: IEEE Computer Society Conference on Computer Vision and Pattern Recognition. Vol. 1. 1999, p. 125â131. [135]R. Siegwart, I. R. Nourbakhsh, and D. Scaramuzza. Introduction to Au- tonomous Mobile Robots. MIT Press, 2011. 8 Classical Methods for Perception The previous chapters focused on using camera models to identify the relation- ship between points in a 3D scene and their projections onto the camera image, as well as how to leverage those models to reconstruct 3D scene structure from images. In this chapter, we introduce methods for extracting various types of in- formation from images through both low-level image processing and higher-level feature extraction techniques. We begin with image processing fundamentals, including filtering, feature de- tection, and description in Section 8.1. We then discuss geometric feature extrac- tion methods in Section 8.2 for identifying structure in sensor data. Finally, in Section 8.3, we cover feature-based object detection approaches and discuss how classical perception methods remain relevant in modern robotics applications. 8.1 Image Processing Fundamentals At its core, image processing is a form of signal processing where the input signal is an image, such as a photo or a video, and the output is either an image or a set of parameters associated with the image. Extracting visual content from raw images is important for mobile robots to be able to intelligently interpret their surroundings 1 . While a large number of image processing techniques exist, in 1 Information extracted through image processing can have a significant im- pact on a robotâs ability to perform fun- damental tasks including localization, mapping, and decision making. this chapter, we focus on some of the more fundamental methods that are relevant for robotics 2 . 2 R. Siegwart, I. R. Nourbakhsh, and D. Scaramuzza. Introduction to Autonomous Mobile Robots. MIT Press, 2011 8.1.1 Image Filtering Image filtering is one of the principal tasks in image processing. The term filter comes from frequency domain signal processing and refers to the process of ac- cepting or rejecting certain frequency components of a signal 3 . Perhaps the most 3 Forexample,eliminatinghigh- frequency noise is a classic filtering problem. common type of image filtering is spatial filtering. The basic principle of spatial filtering is that a particular pixel is modified in the filtered image based on the pixels in the immediate spatial neighborhood, as we show in Section 8.1.1. Mathematically, we describe an image as a function, I(x, y), that maps a pixel at coordinate (x, y) in the domain [a, b]Ă [c, d] to either a scalar for grayscale im- This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 204 principles of robot autonomy ages or a three-dimensional vector corresponding to red, green, and blue values for color images. A spatial filter for an image, I(x, y), consists of a neighbor- hood of pixels around a particular point, (x, y), under examination, which we denote as S xy 4 , and a predefined operation, F, that is performed on the image 4 This region is typically rectangular. pixels encompassed by the neighborhood S xy . We define a new image, I âČ (x, y), by applying the spatial filter operation F to all pixels, (x, y), in the original image, I. Figure 8.1:Illustration of the concept of spatial filtering. The spatial filter operates on a neigh- borhood, S xy , of each point in the original image to produce a new pixel in the filtered image. In general, filters can leverage linear or nonlinear operations, but many of the most fundamental filters are linear and we can express them mathematically as: I âČ (x, y) = F⊠I = N â i=âN M â j=âM F(i, j)I(x + i, y + j),(8.1) where N and M are integers that define the width and height of a rectangular neighborhood, S xy . Based on the size of this neighborhood, we say that this filter is of size (2N + 1)Ă (2M + 1). We generally refer to the filter operation F as a mask or kernel. Broadly speaking, we refer to filters expressed by Equation (8.1) as correlation filters. Convolution filters are another class of linear filters that we commonly use. Convolution filters are similar to correlation filters, but use reverse image in- dices 5 . In particular, we express convolution filters mathematically by: 5 In fact, correlation and convolution fil- ters are identical when the filter mask is symmetric in both the horizontal and vertical directions. I âČ (x, y) = Fâ I = N â i=âN M â j=âM F(i, j)I(xâ i, yâ j).(8.2) Convolution filters are associative, meaning that for two different filter masks, F and G, it is true that Fâ (Gâ I) = (Fâ G)â I. This associative property is useful for tasks such as smoothing an image before applying a differentiation filter. Suppose the mask F implements a derivative filter and G implements a smoothing filter, then sequentially applying these filters would result in Fâ (Gâ I). However, because of the associative property, we can convolve the masks together first such that only the single filter (Fâ G)â I needs to be applied to the image. Note that in both correlation and convolution filters, the boundaries of the image need some special care because of the width and height of the mask. For example, in Section 8.1.1 we show how the filtered image is smaller than the classical methods for perception 205 original due to the width and height of the mask. Some possible options to handle this include padding the image, cropping it, extending it, or wrapping it. However, as images are generally quite large relative to the mask size, the exact approach likely wonât vary the final result significantly. Figure 8.2: Due to the width and height of the mask, the fil- tered image may be smaller than the original. This can be fixed with several techniques, such as padding. Example 8.1.1 (Practical tricks for image filtering). When implementing correla- tion and convolution filters, we can leverage special tricks to simplify the process. In this example, we introduce two simplification tricks: a change in indexing and zero-padding. First, to accommodate varying sizes of filters, including even and odd sized fil- ters, we can change the indexing such that the coordinate of interest is associated with the top-left element in the window rather than the center. For a correlation filter, this would correspond to: I âČ (x, y) = F⊠I = Kâ1 â i=0 Lâ1 â j=0 F(i, j)I(x + i, y + j),(8.3) where K and L are integers that define the width and height of the filter, and the pixel (x, y) is at row x and column y. Note that this formulation results in an output image, I âČ , that is shifted up and to the left. To see this shift, consider the top-left pixel at x = 0 and y = 0 in the new image, I âČ . We generate this new pixel value by applying the filter, F, over the pixels in the original image at rows 0, . . . , Kâ 1 and columns 0, . . . , Lâ 1, which is not centered at (0, 0) in the original image, I. In practice, this shifting is not an issue as long as we always index with respect to the top-left corner. We show an example of top-left indexing in Example 8.1.1. Zero-padding 6 is another simple trick that we can use to ensure that the output 6 Also commonly referred to as same padding. filtered image, I âČ , has the same dimension as the input image, I. In this approach, we pad the left and right boundaries of the image byâK/2â columns of zeros, and pad the top and bottom boundaries by âL/2â rows of zeros, where â·â denotes the floor operation. For example, the image: I = ïŁź ïŁŻ ïŁ° 1 2 3 4 5 6 7 8 9 ïŁč ïŁș ïŁ» , 206 principles of robot autonomy Figure 8.3:Top-left indexing is typically easier to implement than center indexing.Notice that when top-left indexing, it appears as if the filtered image has shifted with respect to when we use center indexing. becomes: I padded = ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° 0 0 0 0 0 0 1 2 3 0 0 4 5 6 0 0 7 8 9 0 0 0 0 0 0 ïŁč ïŁș ïŁș ïŁș ïŁș ïŁș ïŁ» , for filters F â R 3Ă3 , F â R 2Ă2 , F â R 2Ă3 and F â R 3Ă2 . When using this padding rule with a correlation filter from Equation (8.3) and a filter, F, with K = 2, 3 and L = 2, 3, we can define the new image, I âČ , for values x â 1, 2, 3 and y â 1, 2, 3, resulting in I âČ being the same dimension as the original image, I. We show an example use of padding combined with top-left indexing graphically in Example 8.1.1. Figure 8.4: Image padding is a commonly used technique to en- sure that the size of the filtered image is the same size as the original. Moving average filter. The moving average filter returns the average of the pixels in the mask, which achieves a smoothing effect 7 . For example, we can define a 7 Smoothing removes sharp features in the image. moving average filter with a normalized 8 3Ă 3 mask with F from Equation (8.1) 8 The normalization is used to maintain the overall brightness of the image. defined as: F = 1 9 ïŁź ïŁŻ ïŁ° 1 1 1 1 1 1 1 1 1 ïŁč ïŁș ïŁ» . Due to the symmetry of the mask, the correlation filter from Equation (8.1) and convolution filter from Equation (8.2) will be identical. classical methods for perception 207 Gaussian smoothing filter. Gaussian smoothing filters are similar to the moving average filter, but instead of weighting all of the pixels evenly they are weighted by the Gaussian function: G Ï (x, y) = 1 2ÏÏ 2 exp â x 2 + y 2 2Ï 2 . We use this function to obtain the mask operation, F, by sampling the function about the center pixel. For example, for the center pixel with i = j = 0 in Equation (8.1), we sample G Ï (0, 0). For a normalized 3Ă 3 mask with Ï = 0.85, this filter is approximately defined by: F = 1 16 ïŁź ïŁŻ ïŁ° 1 2 1 2 4 2 1 2 1 ïŁč ïŁș ïŁ» . Like the moving average filter, this filter mask is symmetric and therefore yields identical results with respect to the correlation or convolution filters. The ad- vantage of the Gaussian filter is that it provides more weight to the neighboring pixels that are closer. We show an example of this filter in Section 8.1.1. Figure 8.5: Example of a Gaus- sian smoothing filter, which pro- duces a smoothing (blurring) ef- fect on the filtered image. Separable masks. We call a mask separable if it can be broken down into the con- volution of two kernels, F = F 1 â F 2 . If a mask is separable into smaller masks, then it is often cheaper to apply F 1 followed by F 2 , rather than by F directly. One special case of this is when we can represent the mask as an outer product of two vectors, meaning it is equivalent to the 2D convolution of those two vectors. If a separable mask has shape MĂ M and the input image has size wĂ h, then the computational complexity of directly performing the convolution is O(M 2 wh). By separating the masks, the computational cost is O(2Mwh), which is linear in M rather than quadratic. As an example, consider the moving average filter mask from before: F = 1 9 ïŁź ïŁŻ ïŁ° 1 1 1 1 1 1 1 1 1 ïŁč ïŁș ïŁ» = 1 9 ïŁź ïŁŻ ïŁ° 1 1 1 ïŁč ïŁș ïŁ» h 1 1 1 i . 208 principles of robot autonomy As another example, we note that the Gaussian smoothing filter mask is also separable. To see why this is, note that we can decompose the Gaussian weighting function as: G Ï (x, y) = 1 2ÏÏ 2 exp â x 2 + y 2 2Ï 2 = 1 â 2ÏÏ exp â x 2 2Ï 2 1 â 2ÏÏ exp â y 2 2Ï 2 = g Ï (x)· g Ï (y). Image differentiation filters. We can identify some image features, such as edges, by looking at the spatial derivatives in the pixel intensity values in both the verti- cal and horizontal directions. Since we represent images as functions defined over a discrete domain, the traditional method for differentiating continuous functions is not applicable. Instead, we can compute differences between pixels using tech- niques like the central difference method: â I âx = I(x + 1, y)â I(xâ 1, y) 2 , â I ây = I(x, y + 1)â I(x, yâ 1) 2 . (8.4) where â I/âx is the derivative in the horizontal direction and â I/ây is the deriva- tive in the vertical direction. We can also define the derivatives using just one side instead of a central difference, for example â I âx = I(x + 1, y)â I(x, y). We can also differentiate an image using convolution filters. In particular, one common approach is to use a convolution filter of the form Equation (8.2) defined with a mask, F, called a Sobel mask 9 . We denote this mask as S x for the x direction 9 Also referred to as simply a Sobel opera- tor. and S y for the y direction: S x = ïŁź ïŁŻ ïŁ° 1 0 â1 2 0 â2 1 0 â1 ïŁč ïŁș ïŁ» ,S y = ïŁź ïŁŻ ïŁ° 121 000 â1 â2 â1 ïŁč ïŁș ïŁ» .(8.5) Sobel masks are similar to the central difference method but use more neighbor- ing pixels when calculating the derivative 10 . Note that Sobel masks are separable. 10 Specifically, they also consider the rows above and below to compute the difference. Similarity measures. We can also use filtering to find similar features in different images, which can be useful for solving the correspondence problem in stereo vision or structure-from-motion techniques. In particular, we can compute the similarity between the pixel (x, y) in image I 1 and pixel (x âČ , y âČ ) in image I 2 by: S AD = N â i=âN M â j=âM |I 1 (x + i, y + j)â I 2 (x âČ + i, y âČ + j)|, SSD = N â i=âN M â j=âM [ I 1 (x + i, y + j)â I 2 (x âČ + i, y âČ + j)] 2 , (8.6) classical methods for perception 209 where SAD is an acronym for sum of absolute differences, SSD is an acronym for sum of squared differences, and N and M define the size of the window around the pixels that we consider. 8.1.2 Image Feature Detection A local feature 11 in an image is a pattern that differs from its immediate neigh- 11 Also sometimes referred to as interest points, interest regions, or keypoints. borhood in terms of intensity, color, or texture. We can generally categorize local features in several ways, for example by whether or not they provide semantic content. For example, features that may provide semantic content include edges or other geometric shapes, such as lanes of a road or blobs corresponding to blood cells in medical images. Features that do not provide semantic content may also be useful, for example in feature tracking, camera calibration, 3D reconstruction, image mosaicing, and panorama stitching. In these cases, it may be more impor- tant that the feature be able to be located accurately and robustly over time. A third category of features are those that may not have semantic interpretations individually, but may have meaning as a collection. For instance, we could rec- ognize a scene by counting the number of feature matches between the observed scene and a query image. In this case, only the number of matches is relevant and not the location or type of feature. Applications where these types of features are important include texture analysis, scene classification, video mining, and image retrieval. We discuss several feature detection strategies below. While many strategies exist for different types of features, our focus will be on two common features that are often useful in robotics: edges and corners. Edge detection. An edge in an image is a region where there is a significant change in intensity values along one direction, and negligible change along the orthogo- nal direction. In one dimension an edge corresponds to a point where there is a sharp change in intensity, which mathematically can be thought of as a point of a function having a large first derivative and a small second derivative. Many edge detectors rely on this concept by differentiating images and looking for spikes in the derivative. We can evaluate an edge detector based on several criteria for ro- bustness and performance, including accuracy, localization, and single response. Good accuracy implies few false positives or negatives 12 , good localization im- 12 In this case, a false positive is a detec- tion of an edge that isnât real, and a false negative is a missed edge. plies that the detected edge should be exactly where the true edge is in the image, and a single response implies that only one edge is detected for each real edge. Noise and discretization effects can make edge detection challenging in practice. Most edge detection methods rely on two key steps: smoothing and differen- tiation. We perform differentiation in both the vertical and horizontal directions to find locations in the image with high intensity gradients. However, differentia- tion alone is vulnerable to false positives due to image noise, which is why many algorithms will first smooth the image. Example 8.1.2 (Edge detection in 1D). In Example 8.1.2, we show an example of 210 principles of robot autonomy how noise can corrupt image differentiation. Notice that in this case it is impos- Figure 8.6: Differentiation of a signal with noise can be partic- ularly challenging. We can ad- dress this by first smoothing the signal. sible to identify the jump in the signal due to the noise levels. Smoothing filters, such as the Gaussian smoothing filter discussed earlier, can help remedy this problem. In particular, suppose the original signal in Example 8.1.2 is defined by I(x). We can compute a smoothed version by applying a smoothing convolution filter: s(x) = g Ï (x)â I(x), where g Ï (x) represents a Gaussian smoothing filter, and then by applying the differentiation filter: s âČ (x) = d dx â s(x). We show this process in Example 8.1.2. Note that since these filters are convo- lutions, we can leverage the associativity property to combine them into a single filter: s âČ = ( d dx â g Ï )â I. Example 8.1.3 (Edge detection in 2D). Edge detection in a two-dimensional image is quite similar to the example previously discussed for one dimension. Let the smoothing filter be the Gaussian smoothing filter from before, and consider a differentiation filter such as the Sobel filter. We can write the gradient of the smoothed image in both the x and y directions as: âS = " â âx â G Ï â I â ây â G Ï â I # = " G Ï,x â I G Ï,y â I # = " S x S y # , where I is the original image and we use the associativity property of the smooth- ing and differentiation convolution filters to define the combined filters G Ï,x and G Ï,y . We can then compute the magnitude of the gradient by: |âS| = q S 2 x + S 2 y , which we can use to compare against a predefined threshold value for edge de- tection. To guarantee that we define thin edges, it is also possible to filter out classical methods for perception 211 Figure 8.7:Edge detection throughconvolutionwitha Gaussian smoothing filter, fol- lowed by a differentiation filter. points with gradient magnitude above the threshold that are not local maxima. We show an example of this process in Example 8.1.3. Corner detection. A corner in an image is defined as an intersection of two or more edges, and also sometimes as a point where there is a large intensity variation in every direction. Important properties of corner detectors include repeatability and distinctiveness. The repeatability of a corner detector quantifies how well we can find the same features in multiple images even under geometric and photo- metric transformations. Distinctiveness refers to whether the information carried by the patch surrounding the feature is distinctive, which we can use to reliably produce correspondences. Both of these properties are particularly important in applications such as panorama stitching and 3D reconstruction. We can generally think of corner detection in a similar way to edge detection, except that instead of looking for change along one direction there should be changes in all directions. One well-known corner detector is known as the Harris detector 13 , which has the useful property that the detection is invariant to rota- 13 C. Harris and M. Stephens. âA com- bined corner and edge detectorâ. In: 4th Alvey Vision Conference. 1988 tions and linear intensity changes, such as geometric and photometric invariance. However, the Harris detector is not invariant to scale changes or geometric affine 212 principles of robot autonomy Figure 8.8: Edge detection using the Sobel edge detector. changes, which has led to the development of scale-invariant detectors such as the Harris-Laplacian detector or the scale-invariant feature transform (SIFT) detector. 8.1.3 Image Descriptors Image descriptors describe features so that they can be compared across images, or used for object detection and matching. Similar to image detectors, it is desir- able for image descriptors to be repeatable 14 and distinct. Perhaps the simplest 14 For example, invariant with respect to pose, scale, and illumination. example of a descriptor is an nĂ m window of pixel intensities centered at the feature, which we can normalize to be illumination invariant. However, such a descriptor is not invariant to pose or scale and is not distinctive, and therefore is generally not useful in practice. 8.2 Geometric Feature Extraction It is common in robotic localization and mapping to represent the environment using simple geometric primitives 15 that we can efficiently extract from sensor 15 Common geometric primitives include lines, circles, corners, and planes. data. In this section, we present some techniques for line extraction from range data 16 . Lines are one of the most fundamental geometric primitives that we 16 Range data can generally come from a variety of sources, including laser rangefinders, radar, or even computer vision. would want to extract from data, and techniques for extracting other primitives are conceptually similar. There are two main challenges with extracting lines from range data. The first is segmentation, which is the task of identifying which data points belong to which line, and inherently also identifying how many lines there are. The second is fitting, which is the task of estimating the parameters that define a line given a set of points. For simplicity, in this chapter, we consider line extraction problems based on two-dimensional range data. 8.2.1 Line Segmentation The line segmentation problem is to determine how many lines exist in a given set of data and which data points correspond to each line. We will discuss three popular algorithms for line segmentation: the split-and-merge algorithm, the ran- dom sample consensus (RANSAC) algorithm, and the Hough-transform algorithm. classical methods for perception 213 Figure 8.9: Iterative-end-point- fit variation of the split-and- merge algorithm for extracting lines from data. Split-and-merge. The split-and-merge algorithm is a popular line extraction algo- rithm that is fast but not very robust to outliers. The split-and-merge algorithm repeatedly fits lines to sets of points and then splits the set of points into two sets if any point lies more than a specified distance, d, from the line. By repeating this process until no more splits occur, we are guaranteed that all points will lie less than the distance, d, to a line. After this splitting process is complete, a second step merges any of the newly formed lines that are collinear. We present this al- gorithm in more detail in Algorithm 8.1. A popular variant of the split-and-merge Algorithm 8.1: Split-and-Merge Data: Set, S, of N points, distance threshold, d > 0 Result: A list, L, of sets of points, each resembling a line Lââ [S] iââ 1 while i†length(L) do Fit a line (α, r) to the set L[i] Detect the point Pâ L[i] with maximum distance, D, to the line (α, r) if D < d then iââ i + 1 else Split L[i] at P into new sets, S 1 and S 2 L[i]ââ S 1 L[i + 1]ââ S 2 Merge collinear sets in L algorithm is known as the iterative-end-point-fit algorithm. This algorithm is the split-and-merge algorithm in Algorithm 8.1 where the line is constructed by sim- ply connecting the first and the last points of the set. We show this approach graphically in Section 8.2.1. Random sample consensus (RANSAC). Random Sample Consensus (RANSAC) 17 17 M. A. Fischler and R. C. Bolles. âRan- dom sample consensus: a paradigm for model fitting with applications to image analysis and automated cartographyâ. In: Commun. ACM 24.6 (1981), p. 381â 395 is an algorithm to estimate the parameters of a model from a set of data that may contain outliers 18 . Outliers are data points that do not fit the model and 18 This problem is sometimes referred to as robust model parameter estimation. may be the result of high noise in the data, incorrect measurements, or simply points which come from objects that are unrelated to the current model. For 214 principles of robot autonomy example, a laser scan of an indoor environment may contain distinct lines from the surrounding walls but also points from other static and dynamic objects such as chairs or humans. In this case, if the goal is to extract lines to represent the walls, then any data point corresponding to other objects would be an outlier. In general, we can apply RANSAC to many parameter estimation problems, and typical applications in robotics include line extraction from 2D range data, plane extraction from 3D point clouds, and structure-from-motion 19 . We focus on using 19 Where the goal in structure-from- motion problems is to identify image correspondences which satisfy a rigid body transformation. RANSAC for line extraction from two-dimensional data below. RANSAC is an iterative method and is non-deterministic 20 . Given a dataset, S, 20 In other words, it is stochastic or ran- dom. Running the algorithm twice on the same inputs will not necessarily pro- duce the same results. of N points, we start by randomly selecting a sample of two points from S. Next, we construct a line from the two sampled points and compute the distance of all other points to this line. We then define the set of inliers, which is comprised of all points whose distance to the line is within a predefined threshold, d. By repeating this process k times, we generate k inlier sets and their associated lines and return the inlier set with the most points. We detail this procedure in Algorithm 8.2 and illustrate the process in Section 8.2.1. Algorithm 8.2: Random Sample Consensus (RANSAC) for Line Extraction Data: Set, S, of N points, distance threshold, d Result: Set with maximum number of inliers and corresponding line while i†k do Randomly select two points from S. Fit line, l i , through the two points. Compute distance of all other points to l i . Construct set of points, Ì S i , with distance less than d to l i . Store line, l i , and set of points, Ì S i . iââ i + 1 Choose set Ì S i with maximum number of points. Figure 8.10:Example of the RANSAC algorithm, showing four iterations of the algorithm. If the algorithm was terminated after these four iterations, line l 3 would be returned since it con- tains the maximum number of points. Due to the probabilistic nature of the algorithm, as the number of iterations, k, increases the probability of finding a good solution increases. This approach is used over a brute force search of all possible combinations of two points since the total number of combinations is N(Nâ 1)/2, which can be extremely large. In fact, we can perform a simple statistical analysis of RANSAC. Let p be the desired probability of finding a set of points free of outliers and let w be the probability classical methods for perception 215 of selecting an inlier from the dataset, S, of N points, which we can express as: w : = # inliers N . Assuming we draw point samples independently from S, the probability of draw- ing two inliers is w 2 , and 1â w 2 is the probability that at least one is an outlier. Therefore, with k iterations, the probability that RANSAC never selects two points that are both inliers is (1â w 2 ) k . We can therefore find the minimum number of iterations, Ì k, needed to find an outlier-free set with probability p by solving: 1â p = (1â w 2 ) k , for k. In other words, we can compute Ì k as: Ì k = log(1â p) log(1â w 2 ) . While the value of w may not be known exactly 21 , we can still use this expression 21 Thereareadvancedversionsof RANSAC that can estimate w in an adaptive online fashion. to get a good estimate of the number of iterations, k, that we need for good results. It is important to note that this probabilistic approach often leads to a much smaller number of iterations than a brute force search through all combinations. We can attribute this to the fact that Ì k is only a function of w and not the total number of samples, N, in the dataset. Overall, the main advantage of RANSAC is that it is a generic extraction method and can be used with many types of features given a feature model. It is also simple to implement and is robust to data outliers. The main disad- vantages are that the algorithm needs to run multiple times to extract multiple features, and there are no guarantees that the solutions will be optimal. Hough transform. In the Hough transform algorithm, each point, (x i , y i ), of the dataset, S, votes for a set of possible line parameters, (m, b), where m is the slope and b is the intercept point. For any given point, (x i , y i ), the candidate set of line parameters, (m, b), that could pass through this point must satisfy y i = mx i + b, which we can also write as: b =âmx i + y i . Therefore, each point, (x, y), in the original space maps to a line, (m, b), in the Hough space, as we show in Section 8.2.1. The Hough transform algorithm ex- ploits this fact by noting that two points on the same line in the original space will yield two intersecting lines in Hough space. In particular, the point where they intersect in the Hough space corresponds to the parameters m â and b â that defines the line passing between the points in the original space, as we show in Section 8.2.1. We can apply this concept to the line segmentation problem by searching in the Hough space for intersections among the lines that correspond to each point, (x, y), in the set, S. In practice, we do this by discretizing the Hough space with 216 principles of robot autonomy Figure 8.11: Each point, (x i , y i ), in the original space maps to a line in the Hough space which describes all possible parame- ters m and b that would generate a line passing through the point (x i , y i ). Figure 8.12: All points on a line in the original space yield lines in the Hough space that inter- sect at a common point. a grid and simply counting for each grid cell the number of lines corresponding to (x i , y i ) points from S that pass through it. We choose local maxima among the cells as lines that âfitâ the data set, S. However, performing a discretization of the Hough space requires a trade-off between range and resolution, in particular because the slope, m, can range from ââ toâ. Alternatively, we can use a polar coordinate representation of the Hough space which defines a line as: x cos α + y sin α = r, where (α, r) are the new line parameters. With this representation, we map a point, (x i , y i ), from the original space to the polar Hough space, (α, r), as a sinu- soidal curve, as we show in Section 8.2.1. We provide an example of the Hough transform using the polar representation in Section 8.2.1. Figure 8.13: Representation of a point, (x i , y i ), in the Hough space when using a polar coor- dinate representation of a line with parameters α and r. 8.2.2 Point Cloud Registration In robotics, another important sensor modality consists of point clouds, which we can obtain from lidar or RGB-D sensors. One important consideration is to align two point clouds, generally to localize a sensor in its surroundings or to merge data from multiple viewpoints into a unified representation. We refer to classical methods for perception 217 Figure 8.14:Example of the Hough transformation using a polar coordinate representation of lines. this alignment problem formally as point cloud registration, which is the task of finding the geometric transformation that best aligns one point cloud to another. The point cloud registration problem can be formulated as follows. Given a source point cloud P : = p 1 , . . . , p N and a reference point cloud Q : =q 1 , . . . , q M , where each p i , q j â R 3 , our goal is to find the rigid transformation consisting of a rotation matrix Râ SO(3) and translation vector tâ R 3 that best aligns P to Q. We can express this mathematically as minimizing the error metric: E(R, t) = N â i=1 â„R p i + tâ q â i â„ 2 ,(8.7) where q â i denotes the closest point in Q to the transformed point R p i + t. This formulation leads to a challenging optimization problem because both the trans- formation parameters and the point correspondences are unknown. Iterative closest point. The Iterative Closest Point (ICP) algorithm 22 is a widely 22 Z. Zhang. âIterative point matching for registration of free-form curves and surfacesâ. In: International journal of com- puter vision 13.2 (1994), p. 119â152 used method for solving the point cloud registration problem. The algorithm alternates between two steps: finding point correspondences and estimating the optimal transformation given those correspondences. We present the complete ICP algorithm in Algorithm 8.3 and illustrate the iterative alignment process in Section 8.2.2. Figure 8.15: Illustration of the ICP algorithm iteratively align- ing a source point cloud to a ref- erence point cloud.The algo- rithm alternates between finding correspondences and estimating the transformation until conver- gence. The correspondence step in ICP requires finding the nearest neighbor in Q for each transformed point in P. Note that while a naive implementation would re- 218 principles of robot autonomy Algorithm 8.3: Iterative Closest Point (ICP) Data: Source point cloud P, reference point cloud Q, initial transformation (R 0 , t 0 ) (optional), convergence threshold Δ Result: Refined transformation (R, t) Initialize (R, t)ââ (R 0 , t 0 ) or (I, 0) if no initial guess provided E prev âââ repeat // Step 1: Find correspondences for each point p i â P do Find closest point q â i â Q to R p i + t // Step 2: Estimate transformation Compute optimal (R new , t new ) that minimizes â N i=1 â„R new p i + t new â q â i â„ 2 // Step 3: Apply transformation (R, t)ââ (R new , t new ) E prev ââ E Eââ â N i=1 â„R p i + tâ q â i â„ 2 until|Eâ E prev | < Δ; quire O(N M) distance computations per iteration, in practice, we can accelerate this using spatial data structures such as KD-trees, which reduce the average com- plexity to O(N log M). However, it is important to note that the nearest neighbor matching can produce incorrect correspondences, particularly when the point clouds are far from alignment or when they have limited overlap. The transfor- mation estimation step computes the optimal rotation and translation given the current correspondences. This has a closed-form solution that we can obtain us- ing singular value decomposition (SVD). First, we compute the centroids of the corresponding point sets: Ì p = 1 N N â i=1 p i , Ì q = 1 N N â i=1 q â i .(8.8) Next, we construct the cross-covariance matrix: H = N â i=1 ( p i â Ì p)(q â i â Ì q) T .(8.9) Computing the SVD of H = UÎŁV T , the optimal rotation is R = VU T , and the optimal translation is t = Ì qâ R Ì p 23 . 23 We must check that det(R) = 1 to en- sure a proper rotation. If det(R) = â1, we negate the column of V correspond- ing to the smallest singular value. The ICP algorithm converges when the change in error between iterations falls below a threshold Δ, or when the change in transformation parameters is suffi- ciently small. The algorithm is guaranteed to monotonically decrease the align- ment error and converge to a local minimum, though the quality of the final alignment is highly sensitive to the initial transformation estimate. In practice, classical methods for perception 219 several strategies can improve ICPâs robustness and performance, including out- lier rejection methods that discard point pairs with distances exceeding a thresh- old to prevent corrupted correspondences from degrading the solution. As ICP remains a local optimization method, it benefits significantly from good initial- ization, which we can obtain from odometry, inertial sensors, or coarse global registration methods. 8.3 Feature-Based Object Detection Another high-level information extraction task that is common in robotics is object recognition. Object recognition is the task of classifying or naming discrete objects in the world, usually based on images or video. This is a particularly challenging task because real-world scenes are commonly made up of many varying types of objects which can appear at different poses and can occlude each other. Ad- ditionally, objects within a specific class can have a large amount of variability, for example breeds of dogs or car models. In this section, we introduce common methods for feature-based object detection, namely template matching and bag of visual words. 8.3.1 Template Matching Template matching 24 is a machine vision technique for identifying parts of an 24 N. Perveen, D. Kumar, and I. Bhard- waj. âAn overview on template match- ing methodologies and its applicationsâ. In:International Journal of Research in Computer and Communication Technology 2.10 (2013), p. 988â995 image that match a given image pattern 25 . This approach has seen success in a 25 Advanced template matching algo- rithms enable finding pattern occur- rences regardless of their orientation and local brightness. variety of applications, including manufacturing quality control, mobile robotics, and more. The two primary components needed for template matching are the source image, I, and a template image, T. Given a source and template image, one approach to template matching is to leverage the linear spatial correlation filters discussed earlier in this chapter. In particular, a naive approach would be to use the normalized template image as a filter mask in a correlation filter. By applying this filter mask to every pixel in the source image, the resulting output would quantify the similarity of that region of the source image to the template. This type of approach is sometimes referred to as a cross-correlation. Another approach based on linear spatial filters would be to leverage the similarity filters that compute the sum of absolute differences (SAD) metric for each pixel in the source image. Regions of the source image similar to the template would correspond to low SAD scores. The disadvantages of these approaches are that they do not handle rotations or scale changes, which are quite common in real-world applications. One solution to the scaling issue in correlation filter based template match- ing is to simply re-scale the source image multiple times and perform template matching on each. We can use this concept, referred to as using image pyramids 26 , 26 R. Szeliski. Computer vision: algorithms and applications. Springer Science & Busi- ness Media, 2010 to accelerate object search by first using a coarser resolution image to localize the object, and then using finer resolution images for actual detection. We can build image pyramids in several ways. One naive approach is to simply elimi- 220 principles of robot autonomy nate some rows and columns of the image. Another approach is to first use a Gaussian smoothing filter to remove high frequency content from the image and then subsample the image. We refer to the sequence of images resulting from this approach as a Gaussian pyramid. 8.3.2 Bag of Visual Words The key idea behind the bag of visual words 27 approach is that we can simplify 27 The model originated in natural lan- guage processing, where we consider texts such as documents, paragraphs, and sentences as collections, or âbagsâ, of words. object representations by considering them as a collection of their subparts 28 , 28 For example, a bike is an object with wheels, a frame, and handlebars. and we refer to the subparts as visual words. In this approach, we search a source image for visual words, and we create a distribution of visual words that we find in the image in the form of a histogram. We can then perform object detection by comparing this distribution to a set of training images. For example, suppose the source image contains a human face and the recognized features included eyes and a nose. Then, by comparing the distribution to training images, we would likely determine that the training images that also have eyes and a nose are also images of faces. 8.3.3 Classical Perception Approaches Today While modern deep learning methods have revolutionized many computer vision tasks, classical perception techniques remain essential in robotics, particularly for specific instance detection and registration tasks. The fundamental methods we have discussedâfeature detection, description, and matchingâcontinue to form the backbone of many practical robotic systems. We will explore modern deep learning approaches to perception in detail in a subsequent chapter, but it is important to understand where classical methods continue to excel. One area where classical methods remain dominant is in instance detection, where the goal is to identify and localize a specific object rather than classify object categories. For example, detecting a particular coffee cup on a cluttered desk, recognizing a specific tool in a manufacturing environment, or localizing a known landmark for robot navigation all benefit from classical approaches. Un- like category-level classification (e.g., âthis is a cupâ), instance detection requires identifying which specific cup among many possible cups. The typical pipeline for instance detection leverages the classical methods we have covered: 1. Feature Detection and Description: Extract distinctive keypoints from both the query image (the specific object to find) and the target image (the scene to search). Scale-invariant detectors like SIFT or its variants remain popular be- cause they are robust to changes in viewpoint, scale, and illumination. 2. Feature Matching: Match features between the query and target images using descriptor similarity. This step identifies potential correspondences between the known object and regions in the scene. classical methods for perception 221 3. Geometric Verification: Use robust estimation techniques like RANSAC to fit a geometric transformation (such as a homography or rigid body transfor- mation) to the matched features. This step filters out incorrect matches and verifies that the pattern of features is geometrically consistent with the object model. 4. Pose Estimation: Once verified correspondences are obtained, estimate the 3D pose of the object relative to the camera, enabling the robot to interact with or manipulate the object. This classical pipeline offers several advantages that keep it relevant today. First, it requires only a small number of example images or even a 3D model of the specific object, whereas deep learning approaches typically require large labeled datasets. Second, it provides explicit geometric reasoning, yielding not just detection but precise pose estimation needed for robotic manipulation. Third, it is interpretableâengineers can inspect features, matches, and geometric fits to understand and debug system behavior. Classical methods also remain important in registration problems, where the goal is to align sensor data from different viewpoints or modalities. Applica- tions include point cloud alignment for 3D reconstruction, image stitching for panoramas, and multi-sensor fusion, which we will discuss in later chapters. The Iterative Closest Point (ICP) algorithm and feature-based registration using RANSAC continue to be workhorses in these domains. However, modern sys- tems increasingly adopt hybrid approaches that combine classical and learning- based methods. For instance, learned feature detectors and descriptors (such as SuperPoint 29 ) can replace hand-crafted features like SIFT while maintaining the 29 D. DeTone, T. Malisiewicz, and A. Ra- binovich. âSuperpoint: Self-supervised interest point detection and descrip- tionâ. In: Proceedings of the IEEE confer- ence on computer vision and pattern recog- nition workshops. 2018, p. 224â236 geometric reasoning framework. Similarly, learned feature matching networks can improve correspondence quality before geometric verification. These hybrid pipelines leverage the strengths of both paradigms: the data efficiency and ge- ometric rigor of classical methods with the representational power of learned features. In summary, while deep learning has transformed object classification and semantic understanding, classical perception methods remain indispensable for tasks requiring precise instance detection, geometric reasoning, and data-efficient operation. Understanding these fundamental techniques is essential for roboti- cists working on manipulation, localization, and any application where knowing exactly which object is where matters more than simply recognizing what cate- gory it belongs to. 8.4 Summary In this chapter, we introduced methods for extracting various types of informa- tion from images and sensor data through a pipeline of low-level image process- ing and higher-level feature extraction techniques. We began with image process- ing fundamentals, covering filtering operations such as Gaussian smoothing for 222 principles of robot autonomy noise reduction and Sobel operators for differentiation and edge detection. We discussed the mathematical principles of correlation and convolution, along with practical implementation tricks like zero-padding. This foundation extended to feature detection, where we explored strategies for identifying semantically use- ful patterns like edges and corners, and the role of descriptors for representing these features. Building on these low-level techniques, the chapter then presented methods for geometric feature extraction, focusing on the challenge of identifying struc- ture like lines in range data. We detailed and compared three core segmentation algorithms: the fast but less robust Split-and-Merge, the robust but stochastic RANSAC, and the model-based Hough Transform. We also introduced the Iter- ative Closest Point (ICP) algorithm as a fundamental tool for point cloud regis- tration. Finally, we transitioned to feature-based object detection, covering clas- sical approaches such as template matching and the Bag of Visual Words model. We concluded by discussing the enduring relevance of these classical perception methods in modern robotics, particularly for precise instance detection and ge- ometric verification tasks, and noted their role in hybrid systems that combine classical pipelines with learned components. To learn more. For a deeper exploration of the topics covered in this chapter, several key resources are available. A comprehensive introduction to the com- puter vision algorithms that underpin robotic perception can be found in Szeliski [145] and 30 . The original papers on key algorithms provide invaluable insight: 30 H. P. Moravec. âTowards automatic vi- sual obstacle avoidanceâ. In: 5th Interna- tional Joint Conference on Artificial Intelli- gence. 1977 Fischler and Bolles [35] for RANSAC, Harris and Stephens [47] for the corner de- tector, and Zhang [166] for ICP. For a deeper dive into feature descriptors, explore SIFT Lowe [86] and its modern learned counterparts like SuperPoint [31]. Finally, for a broader perspective on how these classical techniques are employed in state- of-the-art systems, consult robotics and computer vision texts such as Siegwart, Nourbakhsh, and Scaramuzza [135]. 8.5 Exercises The starter code for the exercises provided below is available online through GitHub. To get started, download the code by running in a terminal window: git clone https://github.com/StanfordASL/pora-exercises.git We denote Problems requiring hand-written solutions and coding in Python withand, respectively. Problem 1: Correlation and Gaussian Smoothing Filters In this exercise, you will explore developing a correlation and Gaussian smooth- ing filter using the top-left indexing approach defined by Equation (8.3). Specifi- classical methods for perception 223 cally, complete the following: 1. First, consider an image and its zero-padded version: I = ïŁź ïŁŻ ïŁ° 7 4 1 8 5 2 9 6 3 ïŁč ïŁș ïŁ» ,I padded = ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° 0 0 0 0 0 0 7 4 1 0 0 8 5 2 0 0 9 6 3 0 0 0 0 0 0 ïŁč ïŁș ïŁș ïŁș ïŁș ïŁș ïŁ» . Compute by hand the resulting image I âČ from applying the following correla- tion filters: (a) F = ïŁź ïŁŻ ïŁ° 0 0 0 0 1 0 0 0 0 ïŁč ïŁș ïŁ» . (b) F = ïŁź ïŁŻ ïŁ° 1 0 0 0 0 0 0 0 0 ïŁč ïŁș ïŁ» . (c) F = ïŁź ïŁŻ ïŁ° 111 000 â1 â1 â1 ïŁč ïŁș ïŁ» . What is this filter doing to the image? Why might this be useful in computer vision? How would this be different than the functionality of the filter: F âČ = ïŁź ïŁŻ ïŁ° â1 0 1 â1 0 1 â1 0 1 ïŁč ïŁș ïŁ» . (d) F = 1 16 ïŁź ïŁŻ ïŁ° 1 2 1 2 4 2 1 2 1 ïŁč ïŁș ïŁ» . What is this filter doing to the image? Why might this be useful in computer vision? How would this be different than the functionality of the filter: F âČ = 1 9 ïŁź ïŁŻ ïŁ° 1 1 1 1 1 1 1 1 1 ïŁč ïŁș ïŁ» . 2. In the file ch08/exercises/correlation _ filter.ipynb, you will now im- plement the correlation filter. Specifically, implement the function correlate _ image using Equation (8.3). Run the provided code to see the result of your implementation for a horizontal and vertical edge detector filter applied to a test image. 224 principles of robot autonomy 3. Also in the file ch08/exercises/correlation _ filter.ipynb, implement the Gaussian smoothing filter from Section 8.1.1 in the function create _ gaussian _ filter. Run the provided code to see the result of your implementation with Ï = 0.5 and Ï = 2. Explain the general impact of varying Ï on the resulting image. Problem 2: Iterative Closest Point (ICP) for Point Cloud Registration In this exercise, you will use the ICP algorithm defined in Algorithm 8.3 to reg- ister two different point clouds in the same reference frame. In the notebook ch08/exercises/icp.ipynb, complete the following: 1. Run the provided code to load two point clouds: one that represents the target âfullâ point cloud, and another that is just a partial point cloud of the same object. The goal will be to determine the transformation that will align the partial point cloud to the target point cloud. 2. The first step before running ICP is to get an initial transformation estimate. To accomplish this we will use the RANSAC algorithm. Explore and run the provided code to run RANSAC to get this initial estimate. 3. Now, you will implement a basic version of the ICP algorithm. Implement the functions nearest _ neighbor and icp. Run the provided code, there should now be a rough alignment between the partial and target point clouds. Note that this basic version of ICP is not necessarily robust to noise and outliers. 4. Use the open3D libraryâs functions to run a more robust version of the ICP algorithm. Run the provided code to see how well the point clouds align with this implementation. classical methods for perception 225 References [31]D. DeTone, T. Malisiewicz, and A. Rabinovich. âSuperpoint: Self-supervised interest point detection and descriptionâ. In: Proceedings of the IEEE con- ference on computer vision and pattern recognition workshops. 2018, p. 224â 236. [35]M. A. Fischler and R. C. Bolles. âRandom sample consensus: a paradigm for model fitting with applications to image analysis and automated car- tographyâ. In: Commun. ACM 24.6 (1981), p. 381â395. [47]C. Harris and M. Stephens. âA combined corner and edge detectorâ. In: 4th Alvey Vision Conference. 1988. [86]D. G. Lowe. âDistinctive Image Features from Scale-Invariant Keypointsâ. In: International Journal of Computer Vision 60.2 (Nov. 2004), p. 91â110. issn: 1573-1405. doi: 10.1023/B:VISI.0000029664.99615.94. [98]H. P. Moravec. âTowards automatic visual obstacle avoidanceâ. In: 5th International Joint Conference on Artificial Intelligence. 1977. [108]N. Perveen, D. Kumar, and I. Bhardwaj. âAn overview on template match- ing methodologies and its applicationsâ. In: International Journal of Research in Computer and Communication Technology 2.10 (2013), p. 988â995. [135]R. Siegwart, I. R. Nourbakhsh, and D. Scaramuzza. Introduction to Au- tonomous Mobile Robots. MIT Press, 2011. [145]R. Szeliski. Computer vision: algorithms and applications. Springer Science & Business Media, 2010. [166]Z. Zhang. âIterative point matching for registration of free-form curves and surfacesâ. In: International journal of computer vision 13.2 (1994), p. 119â 152. 9 Deep Learning Architectures for Perception Modern computer vision has shifted from hand-crafted features to end-to-end learning with deep neural networks. Rather than manually designing feature extractors like SIFT or HOG descriptors, contemporary approaches learn hierar- chical representations directly from data, achieving strong performance across diverse visual tasks. This transformation has been particularly impactful for robotics, where robust visual perception is essential for autonomous operation in complex, unstructured environments. Consider a mobile robot navigating in- doors: it must process RGB images to recognize objects while interpreting 3D LiDAR point clouds to understand spatial layout and plan collision-free paths. An autonomous vehicle must fuse information from multiple cameras and Li- DAR scanners, processing both 2D image grids and irregular 3D point clouds in real-time. These scenarios require neural network architectures that effectively process different data modalities. Convolutional Neural Networks (CNNs) lever- age spatial locality for processing camera images. Transformers use self-attention to capture long-range dependencies and enable transfer learning. For 3D data, point-based networks like PointNet process unordered point clouds directly, while voxel-based architectures discretize 3D space into regular grids. Each architecture provides learned feature representations essential for downstream robotic tasks. In this chapter, we explore fundamental neural network architectures for robotic perception. We begin with CNNs in Section 9.1 and Transformers in Section 9.2 for image processing, then discuss point-based approaches in Section 9.3 and voxel-based approaches in Section 9.4 for 3D sensor data. These architectural foundations serve as building blocks for the detection, segmentation, and scene understanding methods in subsequent chapters. 9.1 Convolutional Neural Networks Convolutional neural networks (CNNs) are a type of deep learning architecture that is very common in the fields of computer vision and image processing. The architecture of a CNN contains a special structure that leverages convolution filters similar to those we discussed in Chapter 8. However, in the context of machine learning, the convolution filters embedded in the structure of a CNN This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 228 principles of robot autonomy are optimized for the desired task and do not require human specification, giv- ing higher performance and reducing the amount of required manual engineer- ing. CNN architectures comprise several main components: convolutional layers, nonlinear activations, pooling layers, and fully-connected layers. In the following sections, we discuss these components in more detail. 9.1.1 Convolution Layers Figure 9.1: A convolution fil- ter being applied to a 3-channel RGB image. One of the main structural concepts that is unique to the architecture of a CNN is the use of convolution layers. Convolution layers exploit the underlying spa- tial locality structure in images by using sliding, learned filters which are often much smaller than the image itself. Mathematically, these filters perform opera- tions in a manner similar to other linear filters used in image processing, such as Gaussian smoothing filters. For a 2D convolution operation, we can express the computation at each output location as: Y i,j = mâ1 â u=0 nâ1 â v=0 X i+u,j+v · W u,v + b,(9.1) where (i, j) represents the spatial position in the output feature map, X is the input image, W is the filter with dimensions mĂ n, and b is the bias term. This operation slides the filter across the input, computing a weighted sum at each position. For multi-channel inputs like RGB images with C channels, the convo- lution extends to: Y i,j = Câ1 â c=0 mâ1 â u=0 nâ1 â v=0 X i+u,j+v,c · W u,v,c + b, where the filter now has dimension mĂ nĂ C and aggregates information across all input channels to produce a single output value at each spatial location. For example, in Section 9.1.1, we show how a filter is applied over an image with three color channels (red, green, and blue), so C = 3. In this case, the filter has dimension mĂ nĂ 3, which is vectorized to a weight vector, w, with 3mn deep learning architectures for perception 229 elements. The stride of the filter describes how many positions it shifts by when sliding over the input. The output of the filter is then passed through a nonlinear activation 1 . 1 Typically a ReLU function. Once we have applied the filter to the entire image, the collection of outputs from the nonlinear activation function creates a new filtered image, which we typically refer to as an activation map, as shown in Section 9.1.1. In practice, a Figure 9.2:The outputs of a convolution filter and activation function applied across an im- age make up a new image, called an activation map. number of different filters are usually learned in each convolution layer, which produces a corresponding number of activation maps as the output 2 . This is 2 Besides the number of filters applied to the input, the width and height of the filter, the amount of padding on the in- put, and the stride of the filter are other hyperparameters. crucial such that each filter can focus on learning one specific relevant feature. We show examples of different filters that might be learned in different convolution layers of a CNN in Section 9.1.1 3 . Notice that the low-level features which are 3 M. D. Zeiler and R. Fergus. âVisual- izing and Understanding Convolutional Networksâ. In: European Conference on Computer Vision (ECCV). Springer, 2014, p. 818â833 learned in earlier convolution layers look a lot like edge detectors, which are more basic and fundamental features, while later convolution layers have filters that look more like actual objects. Figure 9.3: Low-level, mid-level, and high-level feature visualiza- tions in a convolutional neural network from Zeiler and Fergus (2014). In general, using convolution layers to exploit the spatial locality of images provides several benefits. First, parameter sharing applies the same filter parame- ters at all spatial locations, keeping the total number of learned parameters much smaller than fully-connected layers would require. Second, sparse interactions from having filters smaller than the image enable better detection of small, mean- ingful features and improve computational efficiency through fewer operations. Third, convolutional layers are equivariant to translation, meaning that convolving a shifted image produces the same result as shifting the convolution output of the original image 4 , allowing feature detection regardless of position. Finally, 4 However, convolution is not equivari- ant to changes in scale or rotation. convolutional layers can naturally handle images of varying sizes when needed. 230 principles of robot autonomy 9.1.2 Important CNN Components In addition to convolution layers, there are several other important components that make up the architecture of a CNN. Pooling layers. Pooling is the second major structural component in CNNs. Pool- ing layers typically come after convolution layers and their nonlinear activation functions. The primary function of a pooling layer is to replace the output of the convolution layerâs activation map at particular locations with a summary statis- tic from other spatially local outputs. This helps make the network more robust against small translations in the input, helps improve computational efficiency by reducing the size of the input 5 , and is useful in enabling the input images to 5 This occurs because it lowers the reso- lution. vary in size 6 . The most common type of pooling is max pooling, but other types 6 The size of the pooling can be modified to keep the size of the pooling layer out- put constant. also exist, such as mean pooling. A typical max-pooling operation is shown in Figure 9.4. Figure 9.4: Max pooling exam- ple with 2Ă 2 filter and stride of 2. Computationally, both max and mean pooling layers operate with the same filtering idea as in the convolution layers. Specifically, a filter of width, m, and height, n, slides around the layerâs input with a particular stride. The difference between the two comes from the mathematical operation performed by the filter, which as their names suggest are either a maximum element or the mean over the filter. If the output of the convolution layer has N activation maps, the output of the pooling layer will also have N images, since the pooling filter is only applied across the spatial dimensions. Fully connected layers. Downstream of the convolution and pooling layers are fully connected layers. These layers make up what is essentially just a standard neural network, which is appended to the end of the network. The function of these layers is to take the output of the convolution and pooling layers, which we can think of as a highly condensed representation of the image, and perform a classification or regression. Generally, the total number of fully connected layers at the end of the CNN will only make up a fraction of the total number of layers. CNN performance. We can say that a CNN learns how to process images end-to- end because it essentially learns how to perform two steps simultaneously: fea- ture extraction and classification or regression 7 . In contrast, classical approaches 7 In other words, it learns the entire pro- cess from image input to the desired out- put. to image processing use hand-engineered feature extractors. Since 2012, the per- formance of end-to-end learning approaches to image processing have dominated and continue to improve 8 . This continuous improvement has generally been re- 8 In some specific applications, hand- engineered features may still be better. For example, we might use engineer- ing insight to identify a structure to the problem that a CNN could not easily learn. alized with the use of deeper networks with more parameters, and also by com- bining CNN architectures with other techniques such as Transformers. 9.1.3 Notable CNN Architectures Several landmark CNN architectures have significantly advanced computer vi- sion and demonstrated the power of deep learning. deep learning architectures for perception 231 AlexNet (2012). AlexNet was the first deep CNN to achieve breakthrough per- formance on ImageNet, popularizing the use of ReLU activations and dropout regularization. It demonstrated that deeper networks could dramatically outper- form traditional hand-engineered methods, marking a turning point in computer vision. ResNet (2015). ResNet introduced residual connections that allow information to skip layers, enabling the training of much deeper networksâup to 152 lay- ersâwithout suffering from vanishing gradients. ResNet showed that network depth itself could be a key factor in improving performance, establishing resid- ual connections as a fundamental architectural component. YOLO (You Only Look Once). YOLO pioneered real-time object detection by treat- ing detection as a single regression problem rather than a multi-stage classifica- tion task. YOLO demonstrated how CNN architectures can be adapted for var- ious computer vision tasks beyond image classification, proving that speed and accuracy need not be mutually exclusive. These architectures have not only achieved state-of-the-art results in their re- spective domains but have also influenced countless subsequent designs and es- tablished important principles for CNN development. 9.2 Transformers Transformers are deep learning architectures that have been widely applied across various domains, including natural language processing, computer vision, robotics, and more. Compared to CNNs, Transformers enforce fewer structural constraints on the input data. As long as we can organize the input into a set or sequence of tokens, it can be processed by a Transformer-based model. Since the inter- nal structure of the Transformer is purely learned from data rather than hand- engineered, it requires less domain-specific knowledge, which makes it easier to generalize across different data modalities. Transformers are also computa- tionally efficient and scalable due to their ability to be parallelized, allowing for extremely large models with hundreds of billions of parameters to be trained. 9.2.1 Transformer Architecture Fundamentals The core innovation of the Transformer architecture is the self-attention mecha- nism, which allows the model to learn relationships between different elements in the input sequence or set. Unlike CNNs that have built-in spatial inductive biases through convolution operations, Transformers learn all spatial and seman- tic relationships directly from data. This flexibility comes at a cost: Transformers typically require larger datasets to achieve similar performance as architectures with stronger inductive biases, but they can also achieve superior performance when sufficient data is available. 232 principles of robot autonomy Tokens and embeddings. Transformers take inputs in the form of a set or sequence of tokens. A token is a numeric representation of the raw input data, expressed as a vector. The process of converting raw inputs into tokens is called tokeniza- tion, and the specific method depends on the data modality. For language tasks, a token might represent a word, subword, or character, generated through a learned dictionary lookup. For computer vision tasks, a token typically repre- sents a patchâa square subset of the input image. For example, we can convert an image patch of size PĂ P with C color channels into a token vector of size CP 2 by flattening the patch into a one-dimensional vector. We then transform each token into a token embedding vector, which is a high- dimensional latent space representation. The embedding process is typically a learned linear operation that maps tokens into a space where semantically or structurally similar tokens have similar representations. We also add a positional embedding to each token embedding to encode information about the tokenâs po- sition in the sequence or spatial location. This positional information is crucial because the self-attention mechanism itself is permutation-invariant and does not inherently encode order or position. For example, if a sentence places dog before cat, the model needs that ordering to capture the sentenceâs correct meaning. Similarly, in vision tasks, knowing which patch came from the top-left versus bottom-right of an image provides essential spatial context 9 . 9 Positional embeddings can be learned parameters or fixed sinusoidal functions. Learned embeddings are more flexible but require more data, while fixed em- beddings can generalize to sequence lengths not seen during training. Once we have transformed the raw input into embedding vectors, we aggre- gate them as rows of an input matrix, X 0 â R NĂD , where N is the context size 10 10 In general, larger context sizes will give better performance because we can capture more unique information. However, this comes at the cost of increased computational requirements, which scale quadratically with context size due to the attention mechanism. and D is the dimensionality of the embedding space. Self-attention mechanism. The self-attention mechanism is the key component that allows Transformers to learn relationships between all elements in the input. Each embedding vector in X 0 initially represents only a single token and does not contain contextual information from other tokens. For example, when the phrase toy car appears in a sentence, the embedding vector for car does not yet encode the modifier toy. The goal of self-attention is to allow the model to modify each embedding vector based on its relevance to all other tokens in the sequence. Mathematically, scaled dot-product self-attention is defined as: O = Attention(Q, K, V) = softmax QK †â d k ! V,(9.2) where Q â R mĂd k is the query matrix, K â R nĂd k is the key matrix, V â R nĂd v is the value matrix, and O â R mĂd v is the output matrix. The intuition behind this formulation is that the term QK †computes the dot product between all pairs of queries and keys, where a larger dot product indicates that a particular pair are similar or relevant to each other. We then apply the softmax function to each row of the resulting matrix to normalize the rows so that their elements sum to one 11 . Finally, multiplication by the value matrix V produces an output where 11 The division by â d k , where d k is the dimensionality of the keys and queries, helps stabilize gradients during training by preventing the dot products from be- coming too large. each row is a weighted sum of all value vectors, with weights determined by the normalized attention scores. deep learning architectures for perception 233 In the Transformer architecture, we typically define these matrices as Q = XW Q , K = XW K , and V = XW V , where X is the input matrix with each row corresponding to one embedding vector, and W Q , W K , and W V are matrices of learnable parameters. While we present the mathematical form using matrix notation for implementation clarity, it is often easier to reason about the trans- formation of a single embedding vector. Returning to our toy car example, the attention mechanism would modify the car embedding vector into a new vector that captures relevant information from other words in the sentence. The value matrix V provides potential modifications corresponding to each word, and the attention scores determine which modifications to apply. In this case, the rele- vance weighting would likely show a strong match between car and toy, resulting in an updated embedding that represents toy car rather than a generic car. A single attention mechanism is limited in the types of relationships it can cap- ture, constrained by the finite parameter matrices W Q , W K , and W V . To increase the modelâs expressive capacity, Transformers use multi-head attention, which runs multiple attention mechanisms in parallel, each with unique parameter matrices W i Q , W i K , and W i V for i = 1, . . . , h, where h is the number of attention heads. The outputs from all heads are concatenated and multiplied by another learned matrix W O to produce the final output. This allows the model to attend to dif- ferent types of relationships simultaneouslyâfor example, one head might learn syntactic relationships while another learns semantic relationships. Transformer layer components. Each Transformer layer consists of two main com- ponents arranged sequentially: a multi-head self-attention layer followed by a feed-forward network, with both components wrapped in residual connections and layer normalization operations, as shown in Section 9.2.1. Figure 9.5: A single Transformer layer showing the multi-head at- tention block and feed-forward network, each with residual con- nections and layer normaliza- tion from the Illustrated Trans- former (2018). After the multi-head self-attention layer processes the input, we add the atten- tion output back to the input in what is called a residual connection: Ì X = X + MultiHeadAttention(X). This residual connection allows gradients to flow more easily during training and helps the network learn identity mappings when beneficial. We then apply layer 234 principles of robot autonomy normalization to stabilize training: Ì X = LayerNorm( Ì X). Next, we pass the normalized output through a position-wise feed-forward network, typically implemented as a multi-layer perceptron (MLP). This MLP con- sists of two linear transformations with a non-linear activation function 12 in be- 12 The ReLU or GELU activation func- tions are commonly used. tween: FFN(x) = W 2 · ReLU(W 1 x + b 1 ) + b 2 , where the MLP is applied independently to each position (each row of the input matrix). The feed-forward network typically expands the dimensionality in the first layer and then projects back to the original dimension in the second layer, allowing the network to learn complex non-linear transformations of the attention outputs. This component is followed by another residual connection and layer normalization: X âČ = LayerNorm( Ì X + FFN( Ì X)). A complete Transformer architecture consists of multiple such layers stacked in sequence, where the output of one layer becomes the input to the next. The depth of the network (number of stacked layers) is a key hyperparameter that significantly impacts model capacity and performance 13 . Note that the dimen- 13 Modern Transformers can have dozens or even hundreds of layers. For example, GPT-3 has 96 layers, while some vision models use 32 or more layers. sions of the inputs and outputs of each Transformer layer are typically the same (NĂ D), allowing for flexible stacking of arbitrary depth. Final linear and unembedding layer. After passing through all Transformer layers, we must convert the final embedding representations back into a format suitable for the specific task. The unembedding layer transforms the learned embedding vectors into task-specific outputs. For classification tasks, we typically take a single embedding vectorâeither a special classification token or an aggregated representation of all tokensâand pass it through a linear layer followed by a softmax function: o = softmax(xW + b), where x is the selected embedding vector, and W and b are learned parameters. The output o is a probability distribution over the possible classes, where the vector size matches the number of classes and all elements sum to one. For other tasks, the unembedding layer may take different forms. In sequence- to-sequence tasks like machine translation, we apply a linear transformation to each positionâs embedding to predict the next token in the output sequence. In dense prediction tasks like image segmentation, we may upsample the embed- dings back to the original input resolution and apply per-position classification. The specific design of the final layers depends entirely on the task requirements, while the core Transformer layers remain largely the same across different appli- cations. deep learning architectures for perception 235 9.2.2 Vision Transformers (ViTs) Vision Transformers 14 represent the most important Transformer architecture for 14 A. Dosovitskiy et al. âAn Image is Worth 16x16 Words: Transformers for Image Recognition at Scaleâ. In: Inter- national Conference on Learning Represen- tations. 2021 robotics practitioners, serving as the foundation for modern perception pipelines in embodied AI systems. Unlike earlier vision models that required careful hand- engineering of features and architectural components, ViTs enable end-to-end learning from raw images to task-specific outputs. They have become ubiquitous in robotic applications, from object recognition and scene understanding in au- tonomous navigation systems to visual representations for manipulation policies and multi-modal reasoning in household robots. The ability to pre-train ViTs on large-scale image datasets and then fine-tune them for specific robotic tasks with limited data has made them particularly valuable for real-world deployments where collecting task-specific training data is expensive or impractical. Architecture adaptation for images. Vision Transformers adapt the general Trans- former architecture to process images by treating image patches as tokens. Given an input image of size HĂ W with C color channels, ViT divides the image into a grid of non-overlapping patches of size PĂ P, resulting in N = HW P 2 patches. Each patch is flattened into a vector of dimension C· P 2 and then linearly projected to the embedding dimension D through a learned embedding matrix. Common choices for patch size include P = 16 or P = 32, which balance the trade-off between computational cost (smaller patches create more tokens) and the ability to capture fine-grained details 15 . 15 For a standard 224Ă 224 image with P = 16, this creates 14Ă 14 = 196 patch tokens. In addition to the patch embeddings, ViT introduces a special learnable class to- ken, denoted x cls , which is prepended to the sequence of patch embeddings. This class token has no correspondence to any image patch but serves as a global rep- resentation that the network can use to aggregate information from all patches for classification tasks. The class tokenâs embedding after passing through all Transformer layers is used as the input to the final classification head. Vision Transformers use learnable 2D positional embeddings that encode each patchâs spatial location in the original image grid. Unlike 1D positional embeddings used in language models, these embeddings must capture 2D spatial structure. The standard approach uses a separate learned embedding for each position in the patch grid, which is added to the corresponding patch embedding. Alterna- tive approaches include using sinusoidal positional encodings extended to 2D or learning relative positional biases. The complete input to the first Transformer layer is thus: X 0 = [x cls , x p 1 + e pos 1 , x p 2 + e pos 2 , . . . , x p N + e pos N ], where x p i are the linearly embedded patches and e pos i are the positional embed- dings. Training considerations. A key characteristic of Vision Transformers is their data requirements compared to CNNs. Because ViTs lack the built-in inductive biases of convolution operationsâsuch as translation equivariance and local connec- 236 principles of robot autonomy tivityâthey require substantially larger training datasets to achieve comparable performance with similar model sizes. When trained on smaller datasets like ImageNet-1K (1.3 million images), ViTs typically underperform similarly-sized CNNs. However, when pre-trained on larger datasets such as ImageNet-21K (14 million images) or even larger proprietary datasets, ViTs can match or exceed CNN performance, and their performance continues to improve with dataset scale in ways that CNNs do not. This observation has led to a standard training paradigm for ViTs: pre-training on large-scale datasets followed by fine-tuning on smaller, task-specific datasets. Pre-training can be done with supervised learning on labeled image datasets or through self-supervised methods that learn representations from unlabeled images. Self-supervised pre-training methods such as masked image model- ingâwhere random patches are masked and the model learns to predict themâhave proven particularly effective for ViTs and enable training on massive unlabeled image corpora. After pre-training, the model can be fine-tuned on downstream tasks with relatively small amounts of labeled data, often requiring only the final classification layers to be retrained while the Transformer layers remain largely fixed or are fine-tuned with small learning rates. For robotics applications, this pre-training and fine-tuning paradigm is espe- cially valuable. Robotic systems often operate in specialized environments with limited task-specific data, but can leverage visual representations learned from generic internet-scale image datasets. A ViT pre-trained on diverse visual data captures general-purpose features like edge detection, object recognition, and spatial relationships that transfer effectively to robotic tasks like grasping, navi- gation, and scene understanding. Practical considerations. The choice between Vision Transformers and CNNs for robotic applications involves several practical trade-offs. ViTs excel when large pre-trained models are available and when the task benefits from modeling long- range dependencies across the entire image. Their flexibility in handling variable input sizes and their ability to scale to very large model sizes make them attractive for applications where computational resources are available and high accuracy is critical. However, CNNs remain competitive or superior in scenarios with limited data, real-time constraints on resource-constrained hardware, or tasks where strong spatial inductive biases are beneficial. Computationally, ViTs have quadratic complexity with respect to the num- ber of patches due to the self-attention mechanism, making them more expensive than CNNs for high-resolution images. Recent variants like Swin Transformer ad- dress this by using local attention windows and hierarchical structures, combin- ing some of the efficiency benefits of CNNs with the flexibility of Transformers. For robotics practitioners, the decision often comes down to whether pre-trained models are available for the specific visual domain, the computational budget of the deployment platform, and whether the task requires the global reasoning ca- pabilities that attention mechanisms provide. In many modern robotic systems, deep learning architectures for perception 237 hybrid approaches that combine CNN backbones for efficient feature extraction with Transformer layers for high-level reasoning have proven effective, leveraging the complementary strengths of both architectures. 9.3 Point Cloud Processing and Point-Based Networks Robotic systems operating in 3D environments frequently rely on LiDAR sen- sors, depth cameras, and other sensing modalities that produce point cloud data. Point clouds represent 3D data as collections of points in space, where each point is defined by its position p and may include additional attributes such as color, intensity, or surface normals 16 . In autonomous driving, LiDAR sensors capture 16 Commonrepresentationsinclude Cartesian coordinates (x, y, z), spherical coordinates (r, Ξ, Ï),orcylindrical coordinates (r, Ξ, z) depending on the sensor type. millions of points per second to create detailed 3D maps enabling real-time ob- stacle detection and navigation. Unlike images with regular 2D grid structure, point clouds possess unique characteristics requiring specialized neural network architectures. 9.3.1 Point Cloud Characteristics Point clouds differ fundamentally from images in three key ways that shape neu- ral network design. Point clouds are unordered sets with no inherent sequen- tial or spatial ordering. A point cloud with N points can be represented as p 1 , p 2 , . . . , p N where each point p i â R d (typically d = 3). Crucially, any per- mutation of these points represents the same geometric object, requiring neural networks to be permutation invariant. Figure 9.6: LiDAR point cloud data from an autonomous vehi- cle showing cars and road in- frastructure. Point clouds exhibit variable cardinality, containing vastly different numbers of points across samples. While images have fixed resolution (e.g., 224Ă 224 pixels), point clouds might contain anywhere from 1,000 to 100,000 points depending on scanning resolution, sensor distance, and object complexity, posing challenges for batch processing and network design. Section 9.3.1 and Section 9.3.1 illustrate this variability. Point clouds are also sparse and irregular, with points distributed non- 238 principles of robot autonomy Figure 9.7:Point clouds of different objects showing vari- able cardinality: a simple table (fewer points), a detailed chair (more points), and a toy build- ing facade (most points). uniformly throughout 3D space at varying local densities determined by scanning angle, distance, and surface properties. Most 3D space contains no points, and local neighborhood structure around each point varies significantly, contrasting sharply with the dense, regular grid of image pixels. 9.3.2 PointNet Architecture PointNet 17 approaches point cloud processing through several key architectural 17 C. R. Qi et al. âPointnet: Deep learn- ing on point sets for 3d classification and segmentationâ. In: Proceedings of the IEEE conference on computer vision and pat- tern recognition. 2017, p. 652â660 innovations that ensure permutation invariance while extracting meaningful ge- ometric features. The architecture consists of point-wise feature extraction, sym- metric aggregation functions, and spatial transformation components that work together to process unordered point sets effectively. Figure 9.8:PointNet archi- tectureshowingpoint-wise MLPs, transformation networks (T-Net), and symmetric aggre- gation for classification and segmentation tasks, from Qi et al. (2017). Point-wise multi-layer perceptrons. PointNet applies multi-layer perceptrons (MLPs) independently to each point in the cloud. Given N points where each point p i â R 3 represents spatial coordinates, PointNet computes: h i = MLP( p i ), where h i â R k is the learned feature representation. This point-wise processing maintains permutation invariance because the same transformation applies to each point regardless of input order. The MLP consists of fully connected layers with ReLU activations, progressively increasing dimensionality from 3D coordi- nates to higher-dimensional spaces (e.g., 64, 128, 1024 dimensions). The MLP parameters are shared across all points, similar to parameter sharing in CNN fil- ters, but without spatial locality constraints. The overall architecture is shown in Section 9.3.2. deep learning architectures for perception 239 Symmetric aggregation functions. After extracting point-wise features, PointNet must aggregate these features into a single global representation while preserv- ing permutation invariance. This is achieved through symmetric functions that produce the same output regardless of input ordering. The most commonly used symmetric function in PointNet is the element-wise maximum: g = maximize i=1,...,N h i , where the max operation is applied element-wise across all feature vectors h i . This is provably permutation invariant because for any permutation Ï: max i h Ï(i) = max i h i . While alternative symmetric functions like summation or mean could be used, max pooling has the advantage of being selective, allowing the network to focus on the most discriminative features across all points. However, this global aggre- gation approach means that PointNet captures only global features and may miss important local geometric structures, which motivates the hierarchical extensions in PointNet++, which we will discuss later in this section. Transformation networks (T-Nets). To achieve invariance to geometric transforma- tions such as rotation and translation, PointNet incorporates transformation net- works (T-Net) that learn to align point clouds to a canonical orientation. The T-Net is itself a mini-PointNet that predicts a transformation matrix T â R kĂk : T = T-Net( p 1 , p 2 , . . . , p N ). This transformation matrix is then applied to either the input coordinates (in- put transform) or intermediate features (feature transform). For the input trans- form, T â R 3Ă3 aligns the spatial coordinates, while for the feature transform, T â R 64Ă64 normalizes the feature space. To ensure the stability of optimization, a regularization term is added to the loss function that encourages the transfor- mation matrix to be close to orthogonal: L reg =||Iâ T T || 2 F , where||·|| F denotes the Frobenius norm. This regularization prevents the trans- formation from becoming degenerate and helps maintain the geometric proper- ties of the point cloud. 9.3.3 Notable Point-Based Architectures Several landmark point-based architectures have extended PointNetâs core ideas to address its limitations and improve performance on complex 3D understand- ing tasks. 240 principles of robot autonomy PointNet++. While PointNet effectively captures global features, it struggles to learn local geometric patterns due to its reliance on global max pooling. Point- Net++ 18 addresses this limitation by introducing a hierarchical architecture that 18 C. R. Qi et al. âPointnet++: Deep hier- archical feature learning on point sets in a metric spaceâ. In: Advances in neural information processing systems 30 (2017) learns features at multiple scales, similar to how CNNs build hierarchical rep- resentations through multiple convolutional layers. PointNet++ introduces set abstraction layers that recursively apply PointNet to local regions. Given a point cloud with N points, each set abstraction layer samples N âČ representative points (where N âČ < N), groups nearby points around each representative point, and applies a PointNet to extract local features. This process creates a hierarchical pyramid of features, where early layers capture fine-grained local details and later layers capture broader geometric patterns. Set abstraction in PointNet++ consists of three key operations: sampling, group- ing, and feature extraction. Sampling uses farthest point sampling (FPS) to select representative points that provide good coverage of the entire point cloud. Given a set of points, FPS iteratively selects the point that is farthest from all previously selected points, ensuring diverse spatial coverage. Grouping then defines local regions around each selected point using either ball query (all points within ra- dius r) or k-nearest neighbors. This creates local point sets of varying sizes that capture the local geometry around each representative point. Feature extraction applies PointNet to each local region to learn features that capture local geomet- ric patterns while maintaining permutation invariance within each region. For tasks requiring point-wise predictions like semantic segmentation, PointNet++ includes feature propagation layers that upsample features from coarser to finer resolutions. These layers use inverse distance weighted interpolation to propa- gate features from subsampled points back to the original point cloud: f (j) (x) = â k i=1 w i (x) f (jâ1) i â k i=1 w i (x) ,w i (x) = 1 d(x, x i ) p , where f (j) represents features at layer j, d(x, x i ) is the distance between points, and p is typically set to 2. Skip connections between corresponding abstraction and propagation layers help preserve fine-grained details, similar to U-Net archi- tectures in image segmentation. Dynamic graph CNN (DGCNN). An alternative approach to processing point clouds treats them as graph structures, where points serve as nodes and edges are defined based on spatial proximity or learned relationships. Dynamic Graph Convolutional Neural Networks (DGCNN) 19 exemplify this approach by con- 19 Y. Wang et al. âDynamic graph cnn for learning on point cloudsâ. In: ACM Transactions on Graphics (tog) 38.5 (2019), p. 1â12 structing graphs dynamically in feature space rather than just coordinate space. DGCNN applies edge convolution operations that aggregate information from neighboring points: x âČ i = maximize j:(i,j)âE h Ξ (x i , x j â x i ), where x i and x j are feature vectors of connected points, h Ξ is a learnable function (typically an MLP), and the edge set E is dynamically updated based on feature deep learning architectures for perception 241 similarity after each layer. This dynamic graph construction allows the network to capture both geometric and semantic relationships that evolve as features are learned. The edge convolution operation differs from standard graph convolu- tions by explicitly modeling the edge information (x j â x i ), which captures the relative geometric relationships between neighboring points. This approach has shown success in tasks like point cloud classification and part segmentation. 9.3.4 Applications and Limitations After processing through these point-based architecturesâPointNetâs point-wise MLPs and symmetric aggregation, PointNet++âs hierarchical set abstraction lay- ers, or DGCNNâs dynamic graph convolutionsâthe networks produce rich point- wise feature representations that encode both local geometric patterns and global shape information. These learned features serve as inputs for downstream tasks including 3D object detection, semantic segmentation of points into categories like road, building, or vegetation, and instance segmentation for identifying indi- vidual objects. We will explore training methods for these detection and segmen- tation tasks in subsequent chapters. Despite their effectiveness, point-based methods face computational challenges when processing large-scale point clouds. Real-world applications like autonomous driving can generate point clouds with millions of points per frame, making the O(N 2 ) complexity of neighborhood search in DGCNN or the recursive sam- pling in PointNet++ computationally prohibitive. Memory requirements also scale poorly, as each point must be processed individually, leading to irregu- lar memory access patterns that are inefficient on modern GPU architectures. In the following section, we discuss voxel-based and pillar-based approaches that leverage regular grid structures for efficient 3D convolutions. By discretizing 3D space into regular voxels or vertical pillars, these methods can apply standard convolutional operations while maintaining spatial locality and enabling efficient parallel processing. This structured representation trades some geometric preci- sion for computational efficiency and scalability, making it particularly suitable for real-time applications in autonomous driving and robotics where processing speed is critical. 9.4 Voxel-Based 3D Processing In the previous section, we discussed that the computational limitations of point- based methods have motivated the development of grid-based approaches that discretize 3D space into regular structures, enabling the application of efficient convolutional operations. Rather than processing individual points with irregu- lar neighborhoods, voxel-based and pillar-based methods transform point clouds into structured representations where standard CNNs can be applied. This paradigm shift trades some geometric precision for substantial computational advantages, making real-time processing of large-scale point clouds feasible for applications 242 principles of robot autonomy like autonomous driving. By leveraging the regularity of grid structures, these methods can utilize optimized convolution implementations and parallel process- ing capabilities of modern hardware. 9.4.1 Grid-Based Representations Grid-based methods transform irregular point clouds into structured representa- tions by discretizing 3D space into regular units. Two primary approaches have emerged: voxel-based representations that divide space into cubic voxels, and pillar-based 20 representations that use vertical columns extending through the 20 Frequently used in autonomous ve- hicle or navigation domain, where the scene can be viewed as a 2D âmapâ in- stead of a true 3D scene, for computa- tional efficiency. entire height of the scene. Figure 9.9: Comparison of point cloud representations: original point cloud (dots) overlaid on the voxel-based discretization into 3D cubic cells from Kang et al. (2018). The voxel-based representation (Section 9.4.1) creates a full 3D regular grid struc- ture of size LĂ WĂ H by partitioning space into cubic cells of size v l Ă v w Ă v h 21 , 21 In practice, these dimensions are often set to be equal, creating cubic voxels. where each voxel can contain zero or more points from the original point cloud. This approach preserves complete spatial relationships in all three dimensions, enabling rich 3D feature learning through volumetric convolutions. Points within each voxel are aggregated into a single feature representation, typically through operations like mean pooling, max pooling, or learned aggregation functions. In contrast, the pillar-based representation adopts a 2.5D approach, looking at the scene from a birdâs-eye view, and treats vertical columns (âpillarsâ) as the fundamental processing unit. The space is transformed into a 2D grid structure of size LĂ W. Each pillar extends vertically through the entire height range of the point cloud, effectively collapsing the height dimension during initial pro- cessing. This approach is particularly useful in self-driving settings, where the scene processed is often very large and the reduction to 2D significantly improves computational efficiency. Points within each pillar are aggregated while preserv- ing some height information through encoding strategies, but the primary spatial reasoning occurs in the horizontal, birdâs-eye view plane. The choice between these representations involves significant trade-offs in com- putational complexity and spatial information preservation. Voxel-based meth- ods provide richer spatial context by maintaining full 3D neighborhood relation- ships with memory scaling as O(LĂ W Ă H), enabling detection of complex 3D geometric patterns but requiring computationally expensive 3D convolutions. deep learning architectures for perception 243 Pillar-based approaches reduce complexity by projecting the problem into 2D with memory scaling as O(LĂ W), enabling the use of mature 2D CNN architec- tures and optimized implementations, but potentially losing important vertical structure information crucial for multi-level feature detection. Both representa- tions face spatial resolution trade-offs, where finer grids capture more geometric detail at exponentially higher computational cost, and must address sparsity chal- lenges where most grid cells remain empty, motivating the development of sparse convolution techniques. 9.4.2 3D Convolution Fundamentals Once point clouds are discretized into regular grid structures, we can apply con- volutional operations to learn hierarchical feature representations. This section covers the fundamental operations that enable efficient processing of voxelized 3D data. 3D convolution operations. 3D Convolutional Neural Networks extend the suc- cessful principles of 2D CNNs to volumetric data by operating directly on 3D grids of voxels. While 2D convolutions slide filters across height and width di- mensions of images, 3D convolutions add depth as a third spatial dimension, enabling the network to capture spatial relationships to understand the 3D scene. Specifically, a 3D convolution applies a filter of size (k x , k y , k z ) across all three spatial dimensions of the input volume. Mathematically, for an input volume X and filter W, the 3D convolution operation can be expressed as: Y i,j,k = k x â1 â u=0 k y â1 â v=0 k z â1 â w=0 X i+u,j+v,k+w · W u,v,w + b,(9.3) where (i, j, k) represents the spatial position in the output volume and b is the bias term. Note that in practice, both the input X and output Y typically have an additional channel dimension for multi-channel feature maps, and the weight W is a 4D tensor that includes both spatial dimensions and input/output channel dimensions, while the bias b is a vector with one element per output channel. The geometric interpretation is shown in Section 9.4.2. Figure 9.10:A 3D convolu- tion filter sliding across a volu- metric input, showing how the (k x , k y , k z ) filter operates in all three spatial dimensions. Common kernel sizes include 3Ă 3Ă 3 for capturing local 3D patterns and 1Ă 1Ă 1 for channel-wise feature mixing without spatial aggregation 22 . The key 22 Larger kernels like 5Ă 5Ă 5 can cap- ture broader spatial context but signif- icantly increase computational cost due to the cubic scaling of operations. 244 principles of robot autonomy advantage of 3D convolutions over approaches that process 2D slices indepen- dently is their ability to learn features that span multiple depths, such as the full 3D shape of objects or volumetric textures. The receptive field in 3D grows cubi- cally with network depth, allowing deeper layers to capture increasingly global context, though this rapid growth must be balanced against increased computa- tional cost. Sparse convolutions. Real-world point clouds exhibit extreme sparsity when dis- cretized into voxel grids. In many applications such as autonomous driving and indoor scene processing, the majority of 3D space consists of empty air or unoc- cupied regions. Standard dense 3D convolutions waste significant computation on empty space, making them impractical for large-scale applications. Sparse convolutions address this by computing only on occupied voxels and their neigh- borhoods, using efficient data structures to maintain compact representations of non-empty regions 23 . 23 Popularimplementationsinclude spconv for PyTorch/TensorFlow and Minkowski Engine for general sparse tensor operations. Example 9.4.1 (Memory savings in sparse convolutions). A typical autonomous- vehicle LiDAR scene discretized at 10cm resolution over a 100mĂ 100mĂ 10m volume would require: Grid size = 100m 0.1m Ă 100m 0.1m Ă 10m 0.1m = 1000Ă 1000Ă 100, Total voxels = 1000Ă 1000Ă 100 = 100 million voxel features per layer, In contrast, sparse representations store only the occupied voxels, making mem- ory usage proportional to the number of non-empty voxels rather than total grid size. The sparser the scene, the greater the memory savings. 9.4.3 Notable Voxel-Based Architectures Several landmark architectures have demonstrated the effectiveness of grid-based representations for 3D perception tasks, particularly in autonomous driving ap- plications where real-time performance is critical. VoxelNet. VoxelNet 24 was one of the pioneering architectures for processing 24 Y. Zhou and O. Tuzel. âVoxelnet: End- to-end learning for point cloud based 3d object detectionâ. In: Proceedings of the IEEE conference on computer vision and pat- tern recognition. 2018, p. 4490â4499 voxel-based representations. The architecture addresses the challenge of pro- cessing irregular point clouds by first voxelizing them into a 3D voxel grid, then leveraging 3D convolutions to extract features and detect objects. A core compo- nent of VoxelNet is its Voxel Feature Encoding (VFE) layers, which process the variable number of points within each voxel to produce fixed-size feature repre- sentations. The full architecture is shown in Section 9.4.3. Given a voxel contain- ing points p 1 , p 2 , . . . , p n , where each point p i = (x i , y i , z i , r i ) includes spatial coordinates and optional reflectance intensity, the VFE layers apply point-wise multi-layer perceptrons to each point independently: f i = MLP( p i ). deep learning architectures for perception 245 Figure 9.11:VoxelNet archi- tecture from Zhou and Tuzel (2018), showing the complete pipeline from point cloud vox- elization through VFE layers, 3D convolutional middle layers, to the Region Proposal Network for 3D object detection. To capture contextual information within each voxel, VoxelNet augments each point with the centroid of all points in the same voxel. For a voxel containing n points, the centroid is computed as Ì p = 1 n â n i=1 p i , and each point is then rep- resented as the concatenation [ p i , p i â Ì p], providing both absolute and relative spatial information. The VFE layers then aggregate features across all points in the voxel using element-wise max pooling, ensuring permutation invariance: v = maximize i=1,...,n f i , where v is the final voxel-level feature representation. This aggregation step con- verts the variable-sized point sets within each voxel into fixed-size feature vectors suitable for subsequent 3D convolution operations. After voxel feature encoding, VoxelNet applies a series of 3D convolutional middle layers to build hierarchical representations of the scene. These layers fol- low standard 3D CNN design principles, progressively increasing receptive field size while extracting increasingly abstract features. The sparse nature of voxel occupancy makes this stage well-suited for sparse convolution implementations to improve computational efficiency. The 3D convolutional layers aggregate in- formation across neighboring voxels to capture larger geometric structures, build multi-scale representations through progressive downsampling, and prepare fea- tures for the final object detection stage. PointPillars. While VoxelNet processes full 3D voxels, PointPillars 25 takes a dif- 25 A. H. Lang et al. âPointpillars: Fast encoders for object detection from point cloudsâ. In: Proceedings of the IEEE/CVF conference on computer vision and pattern recognition. 2019, p. 12697â12705 ferent approach by using vertical pillars that extend through the entire height of the scene. The key innovation lies in the Pillar Feature Network (PFN), which encodes points within each pillar and then converts the resulting pillar features into a 2D âpseudo-imageâ representation. This transformation allows PointPillars to leverage mature 2D CNN architectures for subsequent processing, rather than computationally expensive 3D convolutions. The pillar-based approach offers significant computational advantages by reducing the problem from 3D to 2.5D, enabling the use of optimized 2D convolution operations and existing hardware 246 principles of robot autonomy Figure 9.12: PointPillars archi- tecture showing pillar feature encoding and 2D CNN back- bone for efficient real-time 3D object detection, from Lang et al. (2019). accelerations designed for image processing. This design choice makes Point- Pillars particularly suitable for real-time applications where computational ef- ficiency is crucial, achieving inference speeds suitable for autonomous driving while maintaining competitive detection accuracy, as shown in Section 9.4.3. Other variants. Building on the success of VoxelNet and PointPillars, several vari- ants have been developed to further improve performance and efficiency. SEC- OND (Sparsely Embedded Convolutional Detection) 26 combines voxel-based pro- 26 Y. Yan, Y. Mao, and B. Li. âSecond: Sparsely embedded convolutional detec- tionâ. In: Sensors 18.10 (2018), p. 3337 cessing with sparse convolution techniques for improved efficiency, significantly reducing computational requirements while maintaining accuracy. Voxel R-CNN extends the voxel-based approach with refined detection stages, while Voxel- Net introduces architectural improvements that further push the boundaries of voxel-based 3D detection performance. Similar to point-based architectures, after processing through these voxel-based architecturesâVoxelNetâs VFE layers and 3D convolutions, or PointPillarsâ pillar encoding and 2D CNNsâthe networks produce rich feature representations that encode geometric patterns and spatial relationships across the scene. These learned features serve as input to Region Proposal Networks (RPNs) that generate 3D bounding box proposals for object detection. The key contribution of these approaches lies in demonstrating that the entire pipelineâfrom raw point cloud processing to 3D object detectionâcan be trained end-to-end, allowing the networks to learn optimal feature represen- tations specifically for the detection task rather than relying on hand-crafted fea- tures. We will explore the details of training object detection networks and de- signing appropriate loss functions in subsequent chapters on object detection and segmentation. 9.5 Summary In this chapter, we explored fundamental neural network architectures that form the backbone of modern robotic perception, which has shifted from hand-crafted features to end-to-end learning from data. We began with Convolutional Neu- ral Networks (CNNs), detailing their core componentsâconvolutional layers, pooling, and fully-connected layersâthat leverage spatial locality and translation equivariance for processing image data. We discussed landmark architectures like AlexNet, ResNet, and YOLO that demonstrated the power of deep, hierarchical deep learning architectures for perception 247 feature learning. We then introduced the Transformer architecture, whose self- attention mechanism captures long-range dependencies without built-in spatial biases. We covered its key elements, including tokenization, positional embed- dings, and multi-head attention, and focused on its adaptation to vision through Vision Transformers (ViTs), which process images as sequences of patches and excel when pre-trained on large datasets. Finally, we addressed the challenge of 3D sensor data by examining point-based networks like PointNet, which use permutation-invariant operations on point sets, and its hierarchical extension, PointNet++. We then discussed voxel-based methods, which discretize point clouds into regular grids to enable efficient 3D convolutions, as seen in Vox- elNet, and pillar-based approaches like PointPillars that project data into a 2D representation for computational efficiency. To learn more. For a deeper exploration of the topics covered in this chapter, several key resources are available. A comprehensive foundation in deep learn- ing concepts relevant to all architectures discussed can be found in Goodfellow, Bengio, and Courville [41]. The seminal paper on the Transformer architecture is presented by Vaswani et al. [154], while its application to vision is detailed in Dosovitskiy et al. [32]. For in-depth studies on 3D perception, the original papers on PointNet [111] and VoxelNet [167] are essential reading. Finally, for a broader perspective on computer vision algorithms that contextualize these learning-based approaches, we refer the reader to Szeliski [145]. 9.6 Exercises The starter code for the exercises provided below is available online through GitHub. To get started, download the code by running in a terminal window: git clone https://github.com/StanfordASL/pora-exercises.git We denote Problems requiring hand-written solutions and coding in Python withand, respectively. Problem 1: Convolutional Neural Network (CNN) In this exercise you will implement a basic convolutional neural network and use it to classify images from the CIFAR-10 dataset. The CIFAR-10 dataset consists of a large number of small RGB images of objects belonging to ten different classes. In the notebook ch09/exercises/cnn.ipynb, complete the following: 1. Run the provided code to load the CIFAR-10 dataset. Take a look at some of the sample images, what is the dimension of each image? 2. Complete the implementation of the SimpleCNN class to define the model ar- chitecture. Specifically, your model should have two convolution layers with 248 principles of robot autonomy ReLU activation and max pooling. Use the provided values to define the pa- rameters of each of the features, such as the convolution kernel size and num- ber of output channels. Following the convolution layers, your model should have two fully connected layers separated by a ReLU activation. Use the pro- vided value to define the dimension of the hidden layer, and you should be able to determine the appropriate size of the first fully connected layer input based on the last convolution layer output size. Additionally, implement the remaining code in the training loop to train your model using the provided criterion and optimizer. Run the provided code to train your model and evaluate the modelâs performance on a test dataset. 3. Run the provided code to display the confusion matrix from the test dataset results. What is this showing you? Are there any surprising results or does this match your intuition? 4. How many parameters does your model have in total? deep learning architectures for perception 249 References [2]J. Alammar. The Illustrated Transformer. https://jalammar.github.io/ illustrated-transformer/. Blog post. Accessed: 2026-04-09. June 2018. [32]A. Dosovitskiy et al. âAn Image is Worth 16x16 Words: Transformers for Image Recognition at Scaleâ. In: International Conference on Learning Repre- sentations. 2021. [41]I. Goodfellow, Y. Bengio, and A. Courville. Deep Learning. http://w. deeplearningbook.org. MIT Press, 2016. [64]Z. Kang et al. âVoxel-Based Extraction and Classification of 3-D Pole-Like Objects From Mobile LiDAR Point Cloud Dataâ. In: IEEE Journal of Se- lected Topics in Applied Earth Observations and Remote Sensing 11 (Nov. 2018), p. 4287â4298. doi: 10.1109/JSTARS.2018.2869801. [75]A. H. Lang et al. âPointpillars: Fast encoders for object detection from point cloudsâ. In: Proceedings of the IEEE/CVF conference on computer vision and pattern recognition. 2019, p. 12697â12705. [111]C. R. Qi et al. âPointnet: Deep learning on point sets for 3d classification and segmentationâ. In: Proceedings of the IEEE conference on computer vision and pattern recognition. 2017, p. 652â660. [112]C. R. Qi et al. âPointnet++: Deep hierarchical feature learning on point sets in a metric spaceâ. In: Advances in neural information processing systems 30 (2017). [145]R. Szeliski. Computer vision: algorithms and applications. Springer Science & Business Media, 2010. [154]A. Vaswani et al. âAttention is All you Needâ. In: Advances in Neural In- formation Processing Systems. Vol. 30. Curran Associates, Inc., 2017. [157]Y. Wang et al. âDynamic graph cnn for learning on point cloudsâ. In: ACM Transactions on Graphics (tog) 38.5 (2019), p. 1â12. [160]Y. Yan, Y. Mao, and B. Li. âSecond: Sparsely embedded convolutional detectionâ. In: Sensors 18.10 (2018), p. 3337. [162]M. D. Zeiler and R. Fergus. âVisualizing and Understanding Convolu- tional Networksâ. In: European Conference on Computer Vision (ECCV). Springer, 2014, p. 818â833. [167]Y. Zhou and O. Tuzel. âVoxelnet: End-to-end learning for point cloud based 3d object detectionâ. In: Proceedings of the IEEE conference on computer vision and pattern recognition. 2018, p. 4490â4499. 10 Object Detection and Recognition For a robot to safely navigate and interact with the world around it, it needs visual understanding capabilities beyond simple image classificationânot only identifying what objects are present in an image, but also determining where they are located. Consider an autonomous vehicle navigating a busy intersection that must detect multiple pedestrians, vehicles, and cyclists while simultaneously un- derstanding which pixels belong to the drivable road surface versus sidewalks or building facades. A household robot organizing a cluttered kitchen must not only detect individual objects like cups and plates, but also understand their precise boundaries to enable careful grasping and placement. These scenarios require three visual understanding tasks essential for robotics. Object detection identi- fies what objects are present and where they are located using bounding boxes. Semantic segmentation classifies every pixel into scene categories like âroadâ or âvegetationâ. Instance segmentation combines both capabilities, identifying indi- vidual object instances and their precise boundaries. Furthermore, many robotics applications require reasoning about full 3D structures, motivating the extension of detection and segmentation to 3D sensor data, using the point cloud and voxel processing architectures from the previous chapter. In this chapter, we will explore methodological developments that tackle these robotics perception tasks. The progression from expensive two-stage detectors to real-time one-stage approaches addresses the need for low-latency decisions in dynamic environments, while efficient 3D processing methods address computa- tional challenges of real-time LiDAR processing. We will demonstrate how the CNN, PointNet, and voxel-based architectures from the previous chapter can be extended to enable robust visual understanding for autonomous robotic systems. In Section 10.1, we will cover the foundations of 2D object detection, including the evolution from two-stage to one-stage detectors. In Section 10.2, we will discuss how to extend these detection paradigms to 3D sensor data. Finally, in Section 10.3, we will explore semantic and instance segmentation methods for both 2D and 3D data. This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 252 principles of robot autonomy 10.1 2D Object Detection Foundations Object detection extends beyond image classification by not only identifying what objects are present in an image, but also determining where they are located. This dual requirementâclassification and localizationâfundamentally shapes the ar- chitectural design of detection systems. Unlike classification networks that output a single prediction per image, detection networks must handle varying numbers of objects at different scales and positions, requiring specialized architectures that can efficiently process these challenges. Definition 10.1.1 (Object detection). Given an input image I, object detection aims to identify all instances of objects from a predefined set of classes C = c 1 , c 2 , . . . , c K and localize each instance with a bounding box. Formally, the output is a set of detections D = (b i , c i , s i ) N i=1 where b i = (x i , y i , w i , h i ) repre- sents the bounding box coordinates, c i âC is the predicted class, and s i â [0, 1] is the confidence score. The evolution of object detection architectures can be broadly categorized into two paradigms: two-stage detectors that separate object localization from classi- fication, and one-stage detectors that perform both tasks simultaneously. Addi- tionally, the choice between anchor-based and anchor-free methods represents a fundamental design decision that affects both training dynamics and inference ef- ficiency. Understanding these foundational concepts provides the framework for extending detection principles to 3D scenarios and multi-modal sensor fusion, which we will explore in subsequent sections. 10.1.1 Two-Stage Detection: R-CNN to Fast R-CNN The two-stage detection paradigm emerged as a natural approach to the object detection problem by decomposing it into two sequential sub-problems: first gen- erating a set of object proposals that likely contain objects, then classifying these proposals while refining their locations. This divide-and-conquer strategy proved highly effective, establishing the foundation for many subsequent detection archi- tectures. R-CNN: establishing the two-stage paradigm. The original R-CNN (Regions with CNN features) architecture 1 established the two-stage paradigm through a three- 1 Proposed by Girshick et al. (2014), R- CNN demonstrated that CNN features could dramatically improve object detec- tion performance when combined with traditional region proposal methods. step process that combined classical computer vision techniques with modern deep learning. The architecture begins with selective search 2 , which generates 2 Selective search is a graph-based seg- mentation algorithm that generates ob- ject proposals by hierarchically grouping superpixels based on color, texture, size, and shape compatibility. approximately 2,000 region proposals per image by identifying regions likely to contain objects based on low-level visual cues. The process is illustrated in Sec- tion 10.1.1. Each proposed region is then warped to a fixed size of 227Ă 227 pixels and processed independently through a pre-trained CNN (originally AlexNet) to ex- tract a 4096-dimensional feature vector. This feature extraction step leverages the powerful representations learned by CNNs on large-scale image classification object detection and recognition 253 Figure 10.1: Selective search hi- erarchical grouping process fig- ure from Uijlings et al. (2013). Starting from initial superpixel segmentation (left), regions are progressively merged based on color, texture, size, and shape compatibility, ultimately gener- ating diverse object proposals of varying scales and locations (right). datasets, transferring this knowledge to the detection task. Finally, these CNN features are fed to class-specific Support Vector Machine (SVM) classifiers for object classification and linear regressors for bounding box refinement. Mathematically, for a region proposal r with extracted CNN features Ï(r), the classification score for class c is computed as: s c (r) = w T c Ï(r) + b c , where w c and b c are the learned SVM parameters for class c. The bounding box regression predicts corrections (âx,ây,âw,âh) to transform the proposal coordinates (x, y, w, h) to better align with the ground truth: âx = w T x Ï(r) + b x , ây = w T y Ï(r) + b y , âw = w T w Ï(r) + b w , âh = w T h Ï(r) + b h . While R-CNN achieved breakthrough detection performance on benchmark datasets, it suffered from significant computational inefficiencies and training complexity. Each of the 2,000 proposals required a separate forward pass through the CNN, making both training and inference extremely slowâprocessing a sin- gle image could take minutes. The multi-stage training process required pre- training the CNN on ImageNet, training SVMs for classification, and training linear regressors for bounding box refinement, making the pipeline complex and difficult to optimize end-to-end. Fast R-CNN: shared computation breakthrough. Fast R-CNN 3 addressed these lim- 3 Introduced by Girshick (2015), Fast R- CNN addressed the computational bot- tlenecks of R-CNN while maintaining the two-stage paradigm and improving detection accuracy. itations through a key architectural innovation: shared computation across all proposals. Instead of processing each proposal independently through the CNN, Fast R-CNN processes the entire input image once through a convolutional back- bone to generate a feature map, then extracts proposal-specific features from this shared representation. The central innovation is the Region of Interest (RoI) pooling layer, which ex- tracts fixed-size feature representations from variable-sized proposal regions on the shared feature map. For a proposal with coordinates (x, y, w, h) on a feature map F with spatial dimensions HĂ W, RoI pooling divides the proposal region 254 principles of robot autonomy into a regular kĂ k grid (typically 7Ă 7) and applies max pooling within each grid cell, as shown in Section 10.1.1. Figure 10.2: RoI pooling mech- anism.A region of interest proposal of arbitrary size (left) is used to apply max pooling within the feature map (right) to produce a fixed-size feature rep- resentation per region (top) for subsequent classification and re- gression layers. RoI i,j = maximize (x âČ ,y âČ )âbin i,j F x âČ ,y âČ , where bin i,j represents the spatial extent of the (i, j)-th grid cell. This operation ensures that regardless of the input proposal size, the output is always a fixed kĂ kĂ d feature tensor, where d is the number of feature channels. Fast R-CNN also unified the training process through a multi-task loss function that jointly optimizes classification and bounding box regression: L = L cls ( p, u) + λ[uâ„ 1]L bbox (t u , v), where L cls ( p, u) = â log p u is the log loss for true class u with predicted class probabilities p, and L bbox is the smooth L1 loss for bounding box regression. The indicator function [u â„ 1] ensures that bounding box loss is only computed for positive examples (background class has u = 0), and λ balances the two loss terms. This architectural change provided substantial improvements in both compu- tational efficiency and detection accuracy. By sharing CNN computation across all proposals, Fast R-CNN reduced training time by an order of magnitude while achieving higher mean Average Precision (mAP) on standard benchmarks. The end-to-end training also eliminated the complex multi-stage optimization pro- cedure, making the system more practical for real-world deployment. How- ever, Fast R-CNN still relied on external region proposal algorithms like selective search, which remained a computational bottleneck and prevented the entire de- tection pipeline from being truly end-to-end learnable. This limitation motivated the development of learnable region proposal methods, which we will explore in the next section. object detection and recognition 255 10.1.2 Learnable Proposals: RPN and Faster R-CNN While Fast R-CNN significantly improved computational efficiency through shared CNN computation, it still relies on external region proposal algorithms like se- lective search. These traditional methods suffered from several fundamental lim- itations: they were computationally expensive, requiring seconds per image; they were not learned from data and thus could not adapt to specific datasets or tasks; and they created a bottleneck that prevented the entire detection pipeline from being optimized end-to-end. The Region Proposal Network (RPN) innovation addressed these limitations by making region proposal generation a learnable component within the detection framework. Limitations of selective search. Selective search and similar traditional proposal methods operate using hand-crafted features and heuristics that remain fixed re- gardless of the detection task or dataset. These algorithms typically generate thousands of proposals per image using expensive graph-based operations, with processing times often exceeding the CNN inference itself. More critically, since these methods are not learnable, they cannot benefit from the supervision avail- able during detection trainingâthey cannot learn which types of regions are most likely to contain objects for a specific application domain. Region proposal network innovation. The Region Proposal Network (RPN) repre- sents a paradigm shift by treating proposal generation as a learned prediction task. The RPN is essentially a fully convolutional network that slides a small network over the convolutional feature map produced by the backbone CNN. At each sliding window position, the RPN simultaneously predicts multiple region proposals using a set of reference boxes called anchors. Anchor box design principles. Anchors serve as reference templates that cover dif- ferent scales and aspect ratios at each spatial location in the feature map. For a feature map of size HĂ W, the RPN generates HĂ WĂ k potential proposals, where k is the number of anchor templates per location. Common anchor designs use 3 scales (e.g., 128 2 , 256 2 , 512 2 pixels) and 3 aspect ratios (e.g., 1:1, 1:2, 2:1), resulting in k = 9 anchors per location. Mathematically, for an anchor centered at position (x a , y a ) with width w a and height h a , the RPN predicts refinements (âx,ây,âw,âh) to produce a final pro- posal: x =âx· w a + x a , y =ây· h a + y a , w = w a · exp(âw), h = h a · exp(âh). The exponential transformation for width and height ensures positive values and provides scale-invariant parameterization. 256 principles of robot autonomy Objectness scoring. Unlike traditional proposal methods that use complex heuris- tics, the RPN performs binary classification to determine âobjectnessââwhether each anchor location contains an object of any class versus background. This objectness score p â is simpler than full multi-class classification but captures the essential information needed for proposal generation. The RPN learns to distin- guish object-like regions from background using the same convolutional features that will later be used for detailed classification. RPN loss function. The RPN is trained using a multi-task loss that combines objectness classification and bounding box regression: L RPN = 1 N cls â i L cls ( p i , p â i ) + λ 1 N box â i p â i L box (t i , t â i ), where L cls is the log loss for binary classification, L box is the smooth L1 loss for box regression, N cls and N box are normalization terms, and λ balances the two losses. The box regression loss is only computed for positive anchors (those with p â i = 1), indicated by the multiplication with p â i . During training, anchors are assigned positive labels if they have Intersection over Union (IoU) > 0.7 with any ground truth box, or if they are the highest IoU anchor for a ground truth box. Anchors with IoU < 0.3 are assigned negative labels, while those with intermediate IoU values are ignored to avoid ambiguous supervision. Faster R-CNN: integration with Fast R-CNN. Faster R-CNN combines the RPN with Fast R-CNN into a single, unified network that shares convolutional features between proposal generation and detection. The architecture consists of a shared CNN backbone (e.g., VGG-16 or ResNet), followed by two sibling branches: the RPN for generating proposals and the Fast R-CNN detection head for classifying proposals and refining their locations. The shared backbone is crucial for computational efficiencyârather than run- ning separate CNNs for proposal generation and detection, both tasks operate on the same feature representation. This sharing also enables the network to learn features that are beneficial for both tasks simultaneously. Training strategies. Training Faster R-CNN requires careful coordination between the RPN and detection components. Initial approaches alternated between train- ing the RPN and the detection network. First, the RPN is trained using ImageNet- pretrained features. Then, the detection network is trained using proposals from the trained RPN, fine-tuning the shared convolutional layers. This process can be repeated, though diminishing returns are typically observed after the first itera- tion. Today, most training of both components is done simultaneously, using a com- bined loss function: L total = L RPN + L Fast R-CNN . object detection and recognition 257 Joint training is more efficient and often achieves better performance, as it allows the RPN and detection network to adapt to each other during learning. Non-maximum suppression and post-processing. After the RPN generates propos- als, Non-Maximum Suppression (NMS) removes redundant detections. The al- gorithm sorts proposals by objectness score and iteratively removes proposals that have high IoU (typically > 0.7) with higher-scored proposals. This reduces the number of proposals fed to the detection stage from thousands to hundreds, improving computational efficiency while maintaining detection quality. The complete Faster R-CNN pipeline processes an image through the shared backbone, generates scored proposals via RPN with NMS post-processing, ex- tracts RoI features for the top proposals, and produces final classifications and refined bounding boxes. This end-to-end learnable system achieved significant improvements in both speed and accuracy over previous two-stage methods, es- tablishing the foundation for modern object detection architectures. 10.1.3 One-Stage Detection: YOLO The Region Proposal Network represented a major breakthrough by making pro- posal generation learnable, but it still required a two-stage pipeline where pro- posals were generated first and then classified separately. This sequential ap- proach, while effective, created computational bottlenecks that limited real-time performance in robotics applications. As autonomous vehicles, drones, and mo- bile robots demanded faster detection systems for dynamic environments, a fun- damental question emerged: could object detection be reformulated to predict bounding boxes and classes directly from image features in a single forward pass? You Only Look Once (YOLO) 4 provided a radical answer to this question. 4 Introduced by Redmon et al. (2016), YOLO revolutionized object detection by demonstrating that competitive de- tection performance could be achieved through direct single-stage prediction, enablingreal-timeperformancefor robotics applications. Rather than decomposing detection into proposal generation followed by clas- sification, YOLO treats object detection as a single regression problem, directly predicting bounding box coordinates and class probabilities from image pixels in one evaluation of the network. This paradigm shift eliminated the computational overhead of generating and processing thousands of proposals, enabling genuine real-time object detection suitable for robotics systems operating in dynamic en- vironments. Core YOLO innovation: grid-based direct detection. YOLOâs central innovation lies in its spatial decomposition of the detection problem through a grid-based ap- proach. The method divides the input image into an SĂ S grid (typically 7Ă 7 for the original YOLO), where each grid cell becomes responsible for detecting objects whose center points fall within that cellâs spatial region. This responsibil- ity assignment creates a natural spatial organization that eliminates the need for separate proposal generation. Each grid cell simultaneously predicts multiple bounding boxes along with 258 principles of robot autonomy their associated confidence scores and class probabilities. The key insight is that this grid-based spatial division provides sufficient spatial coverage while main- taining computational tractabilityârather than evaluating thousands of potential object locations as in proposal-based methods, YOLO evaluates a fixed number of predictions per grid cell, resulting in a manageable total number of predic- tions regardless of scene complexity. The elimination of the proposal genera- tion stage represents more than just a computational optimization; it fundamen- tally changes how the network approaches object detection. Rather than learning to generate good proposals and then classify them, the network must learn to directly map from image features to final detection outputs. This end-to-end learning enables the network to optimize the entire detection pipeline jointly, potentially leading to better coordination between localization and classification components. YOLO architecture and predictions. The YOLO architecture consists of a single CNN backbone followed by fully connected layers that produce the final detection tensor. The original implementation used a modified GoogLeNet architecture as the backbone, processing input images of size 448Ă 448 pixels through convolu- tional layers that progressively reduce spatial resolution while increasing feature depth. The final convolutional features are flattened and processed through fully connected layers to produce a structured output tensor. The networkâs output is a tensor of size SĂ SĂ (BĂ 5 + C), where S is the grid size, B is the number of bounding boxes predicted per cell, and C is the number of object classes. For the original YOLO trained on PASCAL VOC, this results in a 7Ă 7Ă 30 tensor, with B = 2 bounding boxes and C = 20 classes. Each bounding box prediction consists of five values: (x, y, w, h, confidence), where (x, y) repre- sents the box center relative to the grid cell boundaries, (w, h) represents the box dimensions relative to the entire image, and confidence represents the modelâs certainty that the box contains an object. The class predictions are formulated as conditional probabilities P(Class i |Object), representing the probability of each class given that an object is present in the cell. This conditional formulation is crucialâeach grid cell predicts only one set of class probabilities regardless of the number of bounding boxes, reflecting the assumption that each cell is responsible for at most one object class. The final detection confidence for each bounding box is computed by multiply- ing the conditional class probabilities with the bounding box confidence scores: Detection Confidence = P(Class i |Object)Ă P(Object)Ă IoU(pred, truth). This formulation ensures that high detection scores require both confident object presence and accurate localization. Loss function and training. YOLOâs loss function addresses the multi-task nature of the detection problem by combining coordinate regression, confidence pre- diction, and classification into a unified objective. The loss function consists of object detection and recognition 259 several components with different weights to balance their relative importance: L = λ coord S 2 â i=0 B â j=0 1 obj ij [(x i â Ë x i ) 2 + (y i â Ë y i ) 2 ] |z bounding box center coordinates + λ coord S 2 â i=0 B â j=0 1 obj ij [( â w i â p Ë w i ) 2 + ( p h i â q Ë h i ) 2 ] |z bounding box dimensions + S 2 â i=0 B â j=0 1 obj ij (C i â Ë C i ) 2 |z confidence for cells with objects + λ no-obj S 2 â i=0 B â j=0 1 no-obj ij (C i â Ë C i ) 2 | z confidence for cells without objects + S 2 â i=0 1 obj i â câclasses ( p i (c)â Ë p i (c)) 2 |z class probabilities . The loss function uses different weights for different components: λ coord = 5 in- creases the importance of coordinate predictions, while λ no-obj = 0.5 decreases the weight of confidence predictions for cells without objects. The square root transformation for width and height helps the loss function treat errors in small and large boxes more equally, since a small absolute error in a small box repre- sents a larger relative error than the same absolute error in a large box. The indicator function 1 obj ij denotes whether cell i contains an object and bound- ing box j is responsible for that prediction (determined by which predicted box has the highest IoU with the ground truth). This responsibility assignment is crucial for training stability, as it ensures each ground truth object is associated with exactly one predicted bounding box. Training YOLO follows a two-stage approach: the convolutional layers are first pre-trained on ImageNet for classifi- cation, then the entire network is fine-tuned on detection data. The classification pre-training provides the network with strong feature representations that are then adapted for the detection task. During detection training, the learning rate is carefully adjusted to balance the different loss components and ensure stable convergence. Speed versus accuracy trade-offs and robotics impact. YOLOâs architectural design prioritizes computational efficiency, achieving detection speeds that were un- precedented at the time of its introduction. The original YOLO processes im- ages at 45 frames per second (FPS) on contemporary GPU hardware, while a faster variant (Fast YOLO) achieved 155 FPS by using a smaller network archi- 260 principles of robot autonomy tecture. These speeds represent order-of-magnitude improvements over contem- porary two-stage methods like Fast R-CNN, which operated at approximately 7 FPS. However, this speed comes with accuracy trade-offs. YOLOâs grid-based ap- proach struggles with small objects, since multiple small objects within the same grid cell cannot be detected independently. The method also has difficulty with objects that appear in unusual aspect ratios, as the fixed number of bounding box predictors per cell limits the diversity of detectable shapes. Additionally, the coarse spatial quantization imposed by the grid structure can lead to less precise localization compared to methods that can place proposals at arbitrary locations. For robotics applications, these trade-offs often represent acceptable compro- mises. Autonomous vehicles operating in real-time require detection systems that can process sensor data fast enough to support control decisions, even if absolute detection accuracy is somewhat reduced. The âgood enoughâ detection philos- ophy embodied by YOLO aligns well with robotics applications where timely decisions often matter more than perfect perception. The impact of YOLO on the robotics field extends beyond its specific technical contributions. By demon- strating that real-time object detection was achievable with modest computational resources, YOLO democratized object detection for resource-constrained robotics platforms. Mobile robots, drones, and embedded systems could now incorpo- rate sophisticated visual understanding capabilities without requiring expensive computational hardware. The evolution of YOLO through subsequent versions (YOLOv2, YOLOv3, YOLOv4, YOLOv5, and beyond) has addressed many of the original accuracy limitations while maintaining the core computational advantages. Modern YOLO variants incorporate multi-scale feature processing, improved loss functions, and archi- tectural refinements that close much of the accuracy gap with two-stage meth- ods while preserving real-time performance. This progression demonstrates the enduring value of the single-stage detection paradigm for robotics applications where speed and efficiency are paramount. 10.1.4 Transformer-Based Object Detection Transformers have recently been adapted to tackle object detection, and their performance shows several benefits over CNN-based models. Detection Trans- formers (DETR) 5 , illustrated in Section 10.1.4, propose to formulate the object 5 Nicolas Carion et al. âEnd-to-End Ob- ject Detection with Transformersâ. In: Computer Vision â ECCV 2020. Springer International Publishing, 2020, p. 213â 229 detection problem as a direct set prediction problem, largely streamlining the de- tection pipeline through its end-to-end structure. In its most basic form, DETR is an end-to-end Transformer model that takes in images as inputs and predicts a fixed set of potential bounding boxes. DETR removes many hand-designed com- ponents, including âregion proposalâ and ânon-maximum suppressionâ that are commonly used in CNN-based models. Specifically, DETR uses a conventional CNN backbone to learn 2D feature maps from an input image, as shown on the left side of Section 10.1.4. The model then converts the 2D feature maps into a sequence of feature tokens, similarly to object detection and recognition 261 Figure 10.3:Detection Trans- formers (DETR) from Carion, Massa, et al. (2020) Vision Transformers. These tokenized features are further fed into a Transformer encoder, comprised of a stack of self-attention mechanisms and multi-layer per- ceptrons, for further feature encoding. The encoded sequence is processed by a Transformer decoder that relates the feature sequence with a set of âlearnable ob- ject queriesâ. These object queries encode the distribution of object information, including size, location, and category, over an image. Note that Transformer decoders have some key differences from encoders. For example, they can use what we refer to as cross-attention layers and masked at- tention layers. We use cross-attention layers to allow a sequence to get contex- tual information from another sequence, unlike self-attention layers which gather contextual information from within a single sequence. In the context of DETR, this allows the decoder to relate the encoderâs feature embeddings to the object queries, which are two different input sets. Figure 10.4: Detailed DETR ar- chitecture, from Carion, Massa, et al. (2020). Finally, each object query, after absorbing image features, is processed by shared fully connected layers to predict class labels, bounding box centers, and bounding box sizes. A âno objectâ label is assigned to queries without true objects detected, allowing the model to handle a variable number of objects in an image. In contrast to CNN-based object detectors, Transformer-based object detectors do not have one-to-one matching between the prediction set and the ground- truth set. Therefore, a set-based loss is used to produce an optimal bipartite matching between predicted and ground-truth objects, followed by optimizing object-centric (bounding box) losses. 262 principles of robot autonomy 10.2 3D Object Detection While 2D object detection provides valuable information about what objects are present and their approximate locations in images, many robotics applications require understanding the full 3D structure and pose of objects in the physical world. Consider a robotic arm performing pick-and-place operationsâknowing that a cup appears in a specific region of an image is insufficient for grasping; the robot needs the cupâs precise 3D location, orientation, and dimensions to plan a successful grasp trajectory. Similarly, autonomous vehicles must understand the 3D positions and velocities of surrounding cars, pedestrians, and obstacles to make safe navigation decisions in real-world coordinates rather than image pixels. The transition from 2D to 3D detection introduces changes in problem formulation, data representation, and evaluation metrics while preserving many of the core architectural principles developed for 2D detection. Understanding these extensions provides the foundation for building robust 3D detection sys- tems using the point cloud and voxel processing architectures from the previous chapter. 10.2.1 Extending Object Detection to 3D 3D object detection extends the 2D formulation by instead predicting 3D bound- ing boxes to represent objects in three-dimensional space. While 2D detection outputs bounding boxes parameterized by (x, y, w, h) in image coordinates, 3D detection requires additional parameters to specify the objectâs full pose and ex- tent. Examples of 3D object detection in autonomous driving scenarios are shown in Section 10.2.1. Figure 10.5: 3D object detection in a self-driving scene with 3D bounding boxes overlaid on Li- DAR point cloud and camera in- puts. object detection and recognition 263 Definition 10.2.1 (3D object detection). Given 3D sensor data (point cloud, voxel grid, or RGB-D), 3D object detection aims to identify all instances of objects from a predefined set of classes and localize each instance with a 3D bound- ing box. The output is a set of 3D detections D 3D = (b 3D i , c i , s i ) N i=1 where b 3D i = (x, y, z, l, w, h, Ξ) represents the 3D bounding box with center coordinates (x, y, z), dimensions (l, w, h) for length, width, and height, and orientation Ξ. Extending anchor design to 3D space. For 3D anchor design, each anchor is pa- rameterized by seven values: (x a , y a , z a , l a , w a , h a , Ξ a ) representing the center co- ordinates, dimensions, and orientation. Common 3D anchor designs use aspect ratios appropriate for the target object classes, and discrete orientation bins (e.g., 0°, 90°, 180°) to handle rotation invariance. The anchor refinement process follows similar principles to 2D detection, with the network predicting corrections (âx,ây,âz,âl,âw,âh,âΞ) to transform an- chor parameters into final detections: x =âx· l a + x a , y =ây· w a + y a , z =âz· h a + z a , l = l a · exp(âl), w = w a · exp(âw), h = h a · exp(âh), Ξ = Ξ a +âΞ. Similar to 2D setting, the exponential transformation ensures positive dimen- sions, while orientation is handled through additive corrections with appropriate normalization to handle angle wraparound. Two-stage versus one-stage paradigms in 3D. The two-stage and one-stage detection paradigms from 2D systems transfer directly to 3D detection, with each approach offering distinct advantages for different 3D data modalities and applications. Two-stage 3D detectors follow the proposal-then-classification paradigm, first gen- erating 3D object proposals from point clouds or voxel grids, then refining these proposals through dedicated classification and regression heads. This approach works particularly well with point-based representations, where the first stage can identify promising object centers using techniques like Hough voting, and the second stage can perform detailed classification using local point features. One- stage 3D detectors perform classification and localization simultaneously, making them better suited for real-time robotics applications where latency is critical. These methods work well with regular voxel or pillar representations that enable efficient convolutional processing across the entire 3D space. The choice between paradigms often depends on the input data modality: point-based methods natu- rally lend themselves to two-stage approaches due to the irregular nature of point 264 principles of robot autonomy clouds, while voxel-based methods can efficiently implement one-stage detection using 3D CNNs. Non-maximum suppression in 3D. Non-Maximum Suppression extends to 3D by replacing 2D IoU calculations with 3D IoU or Birdâs Eye View (BEV) IoU metrics. 3D IoU computes the overlap between two 3D bounding boxes in full 3D space, accounting for differences in position, size, and orientation: IoU 3D (b 1 , b 2 ) = Volume(b 1 â© b 2 ) Volume(b 1 âȘ b 2 ) . Computing 3D IoU requires determining the intersection volume between two oriented 3D boxes, which is more complex than the 2D case but essential for accurate duplicate removal. For autonomous driving applications, BEV IoU is often preferred as it focuses on the ground plane where most objects interact: IoU BEV (b 1 , b 2 ) = Area(b BEV 1 â© b BEV 2 ) Area(b BEV 1 âȘ b BEV 2 ) , where b BEV represents the projection of the 3D bounding box onto the ground plane. BEV IoU is computationally simpler and often more relevant for naviga- tion tasks. Evaluation metrics for 3D detection. 3D object detection uses specialized metrics that account for spatial dimensions and orientation accuracy. The standard met- ric is 3D Average Precision (AP) computed using 3D IoU thresholds (typically 0.5 and 0.7). For autonomous driving, evaluation often focuses on Birdâs Eye View (BEV) metrics that emphasize horizontal plane accuracy, with benchmarks like KITTI providing difficulty-based analysis. Orientation accuracy is measured through angular error between predicted and ground truth orientations, with some metrics requiring joint spatial and angular tolerance for correct detections. 10.2.2 3D Detection from Point Clouds and Voxel Representations Building effective 3D object detection systems requires leveraging the special- ized architectures for 3D data processing developed in the previous chapter. The choice between point-based and voxel-based representations fundamentally shapes the detection architecture, with each approach offering distinct advantages for different robotics applications. Point-based methods preserve the geometric precision of the original sensor data and handle irregular point distributions nat- urally, making them well-suited for applications requiring precise object localiza- tion. Voxel-based methods trade some geometric precision for computational ef- ficiency by imposing regular grid structures that enable optimized convolutional operations, making them preferred for real-time robotics applications. Rather than being mutually exclusive, these representations often complement each other within detection pipelines. Many successful 3D detection systems object detection and recognition 265 combine the efficiency of voxel processing for initial feature extraction with the precision of point-based refinement for final object localization. Understand- ing how different detection paradigmsâtwo-stage, one-stage, and transformer- basedâcan be adapted to work with these 3D representations provides the foun- dation for building robust detection systems. Leveraging 3D feature representations. The 3D detection architectures we will ex- plore build directly upon the feature extraction capabilities of PointNet, VoxelNet, and PointPillars discussed in the previous chapter. These architectures provide learned feature representations that encode geometric patterns, spatial relation- ships, and semantic information from 3D sensor data. The key insight is that these features can serve as input to detection heads that predict object classifica- tions and 3D bounding box parameters. Point-based representations excel at preserving fine-grained geometric details and handling the irregular structure of sensor data. PointNet++ features cap- ture multi-scale geometric patterns through hierarchical set abstraction, enabling detection of objects at different scales and levels of detail. These features are par- ticularly valuable for detecting small objects or distinguishing between closely spaced instances where geometric precision is critical. Voxel-based representa- tions provide computational advantages through regular grid structures that en- able efficient 3D convolutions and parallel processing. VoxelNet features encode local geometric patterns within voxels while maintaining spatial relationships across the scene. PointPillars features offer a hybrid approach, encoding verti- cal structure within pillars while enabling efficient 2D processing for large-scale scenes. The choice between these representations often depends on the computa- tional constraints and accuracy requirements of the specific robotics application. 10.2.3 Two-Stage 3D Detection: Extending Faster R-CNN The two-stage detection paradigm extends naturally to 3D by first generating object proposals in 3D space, then refining these proposals through dedicated classification and regression networks. This approach works particularly well when combined with the hierarchical feature representations from PointNet++ or the structured features from VoxelNet. The PointRCNN architecture is illustrated in Section 10.2.3. PointRCNN: point-based two-stage detection. PointRCNN demonstrates how the Faster R-CNN paradigm can be adapted to work directly with point cloud data using PointNet++ features. The architecture follows a bottom-up approach where object proposals are generated directly from point-wise features rather than through dense sliding window approaches used in image detection. The first stage leverages PointNet++ hierarchical features to perform point- wise binary classification, identifying points that likely belong to foreground objects versus background. Rather than generating proposals at regular grid 266 principles of robot autonomy Figure 10.6:PointRCNN ar- chitecture showing bottom-up 3D proposal generation from point-wise features, followed by canonical coordinate refinement and final detection heads from Shi et al. (2019). locations, PointRCNN generates 3D proposals centered at high-confidence fore- ground points. This approach is computationally efficient because it only con- siders a subset of points for proposal generation, and it preserves the geometric precision of the original point cloud. For each proposal, the second stage extracts local point features within the proposed 3D region and applies canonical coordi- nate transformation to normalize the local geometry. This transformation aligns the object coordinate system with a canonical orientation, making the subsequent classification and regression tasks more robust to object orientation variations. The canonical transformation is particularly important for 3D detection because objects can appear in arbitrary orientations in the sensor coordinate system. The mathematical formulation for the canonical transformation involves rotat- ing and translating the local point coordinates so that the objectâs principal axes align with canonical directions: p canonical = R â1 ( p local â t), where R is the estimated object rotation and t is the estimated object center. This transformation enables the network to learn object-centric features that are in- variant to the objectâs pose in the world coordinate system. VoxelNet: voxel-based two-stage detection. VoxelNet adapts the two-stage paradigm to work with voxel-based representations by integrating Voxel Feature Encoding with Region Proposal Network concepts. The architecture processes point clouds through VFE layers to generate voxel-wise features, then applies 3D convolu- tional layers to build hierarchical representations across the voxelized space. The proposal generation stage adapts the RPN concept to 3D by sliding 3D anchor templates across the feature volume. At each spatial location in the 3D feature map, the network predicts objectness scores and 3D bounding box refinements for multiple anchor templates covering different object sizes and orientations. The object detection and recognition 267 3D RPN loss combines objectness classification with 3D bounding box regression: L 3D-RPN = 1 N cls â i L cls ( p i , p â i ) + λ 1 N box â i p â i L 3D-box (b i , b â i ), where L 3D-box incorporates losses for all seven parameters of the 3D bounding box: center coordinates, dimensions, and orientation. The second stage performs 3D RoI pooling to extract fixed-size features for each proposal, followed by classification and bounding box refinement. The 3D RoI pooling operation extends the 2D concept by pooling features from 3D re- gions of the feature volume, maintaining spatial relationships in all three dimen- sions. 10.2.4 One-Stage 3D Detection: Direct Prediction One-stage 3D detection methods perform object classification and localization simultaneously, eliminating the separate proposal generation stage. These ap- proaches are particularly well-suited for real-time robotics applications where detection latency must be minimized. CenterPoint: treating 3D objects as points. CenterPoint represents objects as points in Birdâs Eye View (BEV) space and performs detection through keypoint esti- mation, similar to 2D anchor-free methods like CenterNet. The approach builds on PointPillars pillar-based representation to efficiently process large-scale point clouds while maintaining real-time performance. The key insight is that 3D objects can be effectively represented by their center points when projected into BEV space, particularly for autonomous driving sce- narios where objects primarily move on the ground plane. CenterPoint predicts a heatmap in BEV coordinates where peaks correspond to object centers, along with regression maps that predict 3D bounding box parameters for each detected center. The detection pipeline processes point clouds through PointPillars to gen- erate BEV feature maps, then applies 2D convolutional networks to predict center heatmaps and regression targets: Y heatmap = Ï(Conv 2D (F BEV )), Y regression = Conv 2D (F BEV ), where F BEV represents the BEV feature map from PointPillars processing, and Ï is the sigmoid activation for heatmap prediction. The regression targets include 3D center offsets, object dimensions, and orientation angles. The loss function combines center point detection with regression objectives: L CenterPoint = L heatmap + λ reg L regression , where L heatmap uses focal loss to handle the extreme imbalance between center points and background, and L regression uses smooth L1 loss for the continuous regression targets. CenterPoint extends beyond basic detection by incorporating 268 principles of robot autonomy velocity estimation for tracking applications. By processing consecutive frames, the network can predict object velocities directly as part of the regression tar- gets, enabling seamless integration with multi-object tracking systems essential for autonomous navigation. 10.2.5 Transformer-Based 3D Detection Transformer architectures have been successfully adapted to 3D detection by treating object detection as a set prediction problem, eliminating the need for hand-designed anchors and complex post-processing steps like non-maximum suppression. 3DETR: set-to-set prediction in 3D. 3DETR extends the DETR paradigm to 3D object detection by using transformer architectures to directly predict sets of 3D bounding boxes from point cloud or voxel features. The approach uses learnable object queries that attend to 3D scene features through cross-attention mecha- nisms, enabling end-to-end learning from raw 3D data to final detections. The architecture processes 3D input data through feature extraction networks (Point- Net++ for point clouds or 3D CNNs for voxel grids) to generate scene feature representations. These features are then processed by a transformer encoder to build contextual representations that capture long-range dependencies across the 3D scene. The transformer decoder uses a fixed set of learnable object queries to attend to the encoded scene features and predict object detections. Each object query learns to specialize in detecting objects with particular char- acteristics or in specific spatial regions. The cross-attention mechanism allows queries to gather relevant information from across the entire scene, enabling de- tection of partially occluded objects or objects that extend across multiple local regions. The self-attention within the decoder enables queries to coordinate with each other, reducing duplicate detections without explicit post-processing. The fi- nal prediction heads convert each object queryâs representation into 3D bounding box parameters and class predictions: b i = MLP box (q i ), c i = MLP class (q i ), where q i is the i-th object query after transformer processing. The training uses Hungarian matching to establish optimal assignment between predicted and ground truth objects, followed by standard detection losses. The set-based prediction eliminates the need for anchor design, anchor as- signment strategies, and non-maximum suppression, simplifying the detection pipeline while achieving competitive performance. This approach is particularly attractive for complex 3D scenes where traditional anchor-based methods strug- gle with the high-dimensional anchor space and complex object interactions. object detection and recognition 269 10.3 Semantic and Instance Segmentation Segmentation extends object detection by providing pixel-level or point-level un- derstanding of scenes, enabling robots to understand not just where objects are located, but precisely which pixels belong to each object or scene category. While object detection provides coarse spatial understanding through bounding boxes, segmentation offers fine-grained spatial reasoning essential for tasks like au- tonomous navigation on complex terrain, precise robotic manipulation, and de- tailed scene understanding. Section 10.3 illustrates an example of semantic and instance segmentation outputs. Figure 10.7: An input image and the corresponding semantic and instance segmentation outputs. 10.3.1 Semantic Segmentation Semantic segmentation extends image understanding beyond object detection by classifying every pixel in an image into predefined semantic categories, provid- ing dense spatial understanding of the scene. Unlike object detection which out- puts sparse bounding boxes, semantic segmentation produces pixel-level predic- tions that preserve the precise boundaries and spatial extent of different scene elements. This fine-grained understanding is essential for robotics applications where precise spatial reasoning is required. Problem definition and robotics applications. Semantic segmentation performs pixel- level classification without distinguishing between different instances of the same class. For an input image I of size HĂ W, the output is a segmentation map S of the same spatial dimensions, where each pixel (i, j) is assigned a class label S i,j â C from the predefined set of semantic categories. For robotics systems, autonomous vehicles use segmentation to identify drivable road surfaces, distin- guish between different types of terrain, and understand scene layout for path planning. Mobile robots navigating indoor environments use segmentation to identify floors, walls, furniture, and obstacles, enabling more sophisticated spa- tial reasoning for navigation planning. In each case, the pixel-level precision enables robots to make more informed decisions about how to interact with their environment. Fully convolutional networks (FCNs). Semantic segmentation can be viewed as dense classification where standard CNN architectures are adapted to produce 270 principles of robot autonomy spatial output maps rather than single classification scores. Fully Convolutional Networks (FCNs) build on the convolutional feature extraction capabilities of CNNs while replacing the fully connected classification layers with convolutional layers that preserve spatial structure. These models replace the fully connected layers typically used for classification with convolutional layers that can accept images of arbitrary size and produce correspondingly sized output maps. For a CNN backbone that produces feature maps of size H/32Ă W/32 (due to pooling operations), FCN applies 1Ă 1 convolutions to produce class score maps, then up- samples these maps back to the original image resolution. The upsampling pro- cess uses transposed convolutions (also called deconvolutions) to increase spatial resolution: y i,j = â m,n x âi/sâ+m,âj/sâ+n · w m,n , where s is the upsampling stride and w represents the learned transposed con- volution weights. This operation is the mathematical inverse of convolution with stride s, enabling learnable upsampling that can recover spatial details. FCN introduces skip connections that combine features from different layers of the encoder to recover fine-grained spatial information lost during downsam- pling. These connections add feature maps from earlier layers (with higher spatial resolution) to upsampled feature maps from deeper layers (with richer semantic information): F fused = Upsample(F deep ) + F shallow . This fusion enables the network to combine high-level semantic understanding with low-level spatial precision, crucial for accurate boundary delineation in robotics applications. Figure 10.8: U-Net architecture showing the symmetric encoder- decoder design with skip con- nections at multiple scales, en- abling combination of high- resolution spatial information with high-level semantic fea- tures from Ronneberger et al. (2015). U-Net and encoder-decoder architectures. U-Net represents a systematic approach to encoder-decoder architectures that has become foundational for semantic seg- mentation across many domains. The U-Net architecture, illustrated in Sec- tion 10.3.1, consists of a contracting path (encoder) that progressively reduces spatial resolution while increasing feature depth, followed by an expansive path (decoder) that gradually recovers spatial resolution while combining features across scales. For each decoder layer, skip connections concatenate features from object detection and recognition 271 corresponding encoder layer: F (i) decoder = UpConv(F (iâ1) decoder )â F (i) encoder , where â denotes concatenation and UpConv represents upsampling convolu- tion operations. These skip connections preserve fine-grained spatial details that would otherwise be lost during the encoding process. The symmetric design ensures that the decoder has access to features at mul- tiple scales, enabling accurate segmentation of both large objects (captured by deep, low-resolution features) and fine details (preserved through skip connec- tions from high-resolution features). This multi-scale feature combination is par- ticularly important for robotics applications where accurate boundary detection affects safety and task performance. Training loss: classification cross-entropy per pixel. Semantic segmentation networks are trained using pixel-wise classification loss, treating each pixel as an indepen- dent classification problem. The standard loss function is cross-entropy computed across all pixels: L seg =â 1 HW H â i=1 W â j=1 C â c=1 y i,j,c log( Ë y i,j,c ), where y i,j,c is the ground truth one-hot encoding for pixel (i, j) and class c, and Ë y i,j,c is the predicted probability. This formulation treats each pixel independently, enabling efficient batch processing and straightforward optimization. However, pixel-wise cross-entropy can struggle with class imbalance, which is common in robotics scenarios where background pixels often dominate the scene. Various modifications address this challenge, including weighted cross-entropy that assigns different weights to different classes based on their frequency, and focal loss that emphasizes hard examples by down-weighting well-classified pix- els. 10.3.2 Instance Segmentation Instance segmentation combines object detection and semantic segmentation by identifying individual object instances and their precise pixel-level boundaries. Unlike semantic segmentation which treats all objects of the same class identi- cally, instance segmentation distinguishes between separate instancesâfor exam- ple, identifying three individual cars rather than just âcar pixelsâ. This capability is critical for robotics applications where understanding individual objects en- ables targeted interaction and manipulation. Problem definition and distinction from semantic segmentation. Instance segmenta- tion extends semantic segmentation by assigning unique instance identifiers to pixels belonging to distinct objects. For an input image, the output includes both semantic labels and instance masks, where each instance mask M k defines the 272 principles of robot autonomy pixel-level extent of the k-th detected object instance. The key distinction is that semantic segmentation answers âwhat is this pixel?â while instance segmenta- tion answers âwhat is this pixel and which specific object does it belong to?â For a robotic arm grasping objects from a bin, semantic segmentation might iden- tify all pixels as âtool,â but instance segmentation identifies individual wrenches, screwdrivers, and hammers, enabling the robot to select and grasp specific items. Figure 10.9: Mask R-CNN ar- chitecture showing the addition of a mask prediction branch to Faster R-CNN, with RoI Align replacing RoI pooling for im- proved spatial alignment from He et al. (2017). Mask R-CNN: extending detection with segmentation. Mask R-CNN, illustrated in Section 10.3.2, extends Faster R-CNN by adding a segmentation branch that pre- dicts pixel-level masks for each detected object. The architecture maintains the two-stage paradigm: the RPN generates object proposals, and the detection head performs classification, bounding box regression, and mask prediction. The key innovation is RoI Align, which replaces RoI pooling to address spatial misalign- ment issues. While RoI pooling quantizes proposal coordinates to discrete feature map positions, RoI Align uses bilinear interpolation to sample features at exact locations: F aligned (x, y) = â i,j I(i, j)· max(0, 1â|xâ i|)· max(0, 1â|yâ j|). This precise alignment is essential for accurate mask prediction, as small spatial misalignments can significantly degrade segmentation quality. The mask prediction branch applies a small FCN to each RoI-aligned feature to produce a binary mask for the predicted object class. The mask loss is computed only for the predicted class to avoid competition between classes: L mask =â 1 m 2 â i,j [y i,j log( Ë y k â i,j ) + (1â y i,j ) log(1â Ë y k â i,j )], where k â is the predicted class, y i,j is the ground truth mask, and Ë y k â i,j is the predicted mask for class k â . Panoptic segmentation. Panoptic segmentation unifies semantic and instance seg- mentation by providing complete scene understanding. The task divides seman- tic categories into "things" (countable objects like cars, people) and "stuff" (amor- phous regions like sky, road), performing instance segmentation for things and object detection and recognition 273 semantic segmentation for stuff. For robotics applications, panoptic segmentation provides comprehensive scene understanding. An autonomous vehicle can si- multaneously understand the road surface (stuff), individual vehicles and pedes- trians (thing instances), and background elements like buildings and vegetation (stuff), enabling holistic reasoning about the driving environment. Bottom-up approaches. Bottom-up instance segmentation methods first perform pixel-level feature learning, then group pixels into instances based on learned embeddings. These approaches contrast with top-down methods like Mask R- CNN that first detect objects then segment them. Associative embedding learns pixel-level features where pixels belonging to the same instance have similar em- bedding vectors, while pixels from different instances have dissimilar embed- dings. Instance masks are then generated by clustering pixels in the embedding space: d(e i , e j ) =||e i â e j || 2 , where e i and e j are embedding vectors for pixels i and j. Pixels with distances below a threshold are grouped into the same instance. These methods can handle arbitrary numbers of instances without predefined proposals but require robust clustering algorithms to separate instances reliably. They are particularly useful for robotics scenarios with dense object arrangements where proposal-based methods might struggle. 10.3.3 3D Segmentation 3D segmentation extends pixel-level understanding to volumetric data, provid- ing precise spatial reasoning for robotics applications that require detailed 3D scene understanding. While 2D segmentation enables robots to understand im- age content, 3D segmentation allows reasoning about the full spatial extent and structure of objects in the physical world. This capability is essential for manip- ulation tasks requiring grasp planning, navigation in complex 3D environments, and understanding object affordances based on geometric structure. Point cloud segmentation. Point cloud segmentation assigns semantic labels or instance identifiers to individual points in 3D space. The formulation extends 2D segmentation concepts to irregular point data, where each point p i = (x i , y i , z i ) receives a label l i â C for semantic segmentation or instance identifier I i for instance segmentation. Semantic segmentation of point clouds using PointNet++ leverages the hierarchical set abstraction layers from the previous chapter. The architecture processes points through multiple scales of local feature extraction and aggregation, then applies classification heads to predict semantic labels for each point: l i = MLP seg (f (L) i ), where f (L) i represents the final point-wise feature after L layers of hierarchical processing. The multi-scale feature extraction enables accurate segmentation of 274 principles of robot autonomy objects at different sizes and levels of detail. Instance segmentation in point clouds requires additional mechanisms to group points into distinct object instances. Methods like PointGroup combine seman- tic segmentation with learned offset vectors that point toward instance centers, enabling clustering of points belonging to the same object. The training loss com- bines semantic classification with offset regression: L point-instance = L semantic + λL offset + ÎłL clustering , where L offset encourages points to predict vectors pointing toward their instance centers, and L clustering promotes tight clustering within instances and separation between instances. Figure 10.10: 3D voxel-based segmentation example from the Occ3D dataset for self-driving scenes, specifying what object category occupies each 3D loca- tion. Voxel-based segmentation. Voxel-based segmentation processes regular 3D grids where each voxel represents a volumetric unit in 3D space. The formulation treats segmentation as 3D dense classification, where each voxel v i,j,k receives a semantic label or occupancy prediction. Occupancy grids represent a fundamen- tal approach where each voxel indicates whether that region of space is occupied by an object. This binary classification provides essential information for naviga- tion and collision avoidance: o i,j,k = Ï(MLP(f i,j,k )), where o i,j,k â [0, 1] represents the occupancy probability for voxel (i, j, k). 3D U-Net architectures extend the encoder-decoder paradigm to volumetric data for detailed semantic segmentation. The architecture applies 3D convolu- tions throughout the encoding and decoding paths, with 3D skip connections preserving spatial details: F (i) decoder = UpConv3D(F (iâ1) decoder )â F (i) encoder . The 3D convolutions capture volumetric patterns and spatial relationships es- sential for accurate 3D segmentation, while skip connections ensure fine-grained geometric details are preserved in the final predictions. An example of 3D voxel- based segmentation is shown in Section 10.3.3. 10.3.4 Robotics-Specific Applications Segmentation enables several critical robotics capabilities that require detailed ge- ometric understanding of objects and environments. We explore a few examples object detection and recognition 275 below. Example 10.3.1. Grasp point prediction through part segmentation identifies func- tional regions of objects that are suitable for robotic grasping. By segmenting objects into semantic parts (handles, graspable surfaces, fragile regions), robots can plan grasps that are both mechanically sound and functionally appropriate. For example, segmenting a mug into handle, rim, and body regions enables the robot to choose appropriate grasp locations based on the intended manipulation task. Example 10.3.2. Terrain traversability analysis uses 3D segmentation to classify different terrain types and their suitability for robot navigation. Outdoor mobile robots use segmentation to distinguish between solid ground, obstacles, vegeta- tion, and hazardous terrain, enabling safe path planning in complex outdoor en- vironments. The 3D understanding allows reasoning about terrain slope, rough- ness, and stability that would be impossible with 2D analysis alone. Example 10.3.3. Object affordance understanding through part-based analysis enables robots to reason about how objects can be used based on their geomet- ric structure. By segmenting objects into functional parts and understanding the spatial relationships between parts, robots can infer possible interactions and ma- nipulation strategies. A segmented chair with identified seat, backrest, and legs enables the robot to understand both the objectâs function and how to manipulate it safely. These applications demonstrate how 3D segmentation provides the detailed spatial understanding necessary for robots to interact effectively with complex 3D environments, going beyond simple object detection to enable sophisticated reasoning about object structure, function, and manipulation possibilities. 10.4 Summary In this chapter, we explored the essential robotic perception tasks of object de- tection and segmentation, which provide the spatial understanding necessary for robots to interact intelligently with their environments. We began with the foundations of 2D object detection, contrasting the two- stage paradigmâexemplified by the evolution from R-CNN to the efficient, learn- able proposals of Faster R-CNNâwith the one-stage paradigm of YOLO, which prioritizes speed for real-time applications. We also discussed how the Trans- former architecture has been adapted for detection with models like DETR, which simplify the pipeline by framing detection as a direct set prediction problem. We then extended these concepts to 3D, detailing how detection paradigms adapt to point cloud and voxel data. We covered two-stage point-based methods like PointRCNN, one-stage voxel-based methods like CenterPoint, and Transformer- based approaches like 3DETR, each offering different trade-offs between precision and computational efficiency for processing 3D sensor data. 276 principles of robot autonomy Finally, we delved into segmentation, which provides pixel- and point-level understanding. We covered semantic segmentation with architectures like FCN and U-Net for categorizing every pixel, instance segmentation with Mask R-CNN for identifying individual objects, and their unification in panoptic segmentation. We further extended these ideas to 3D point cloud and voxel-based segmenta- tion, highlighting their critical role in applications requiring detailed geometric reasoning, such as grasp point prediction and terrain analysis. To learn more. For a deeper exploration of the topics covered in this chapter, several key resources are available. The seminal papers on two-stage detection [40] and one-stage detection [121] are foundational to the field. For Transformer- based detection, the DETR paper [23] introduced the set prediction paradigm. In 3D perception, the original papers on PointRCNN [129] and VoxelNet [167] are essential reading for point-based and voxel-based detection, respectively. For segmentation, the works on Mask R-CNN [50] and U-Net [124] provide the basis for modern instance and semantic segmentation techniques. 10.5 Exercises The starter code for the exercises provided below is available online through GitHub. To get started, download the code by running in a terminal window: git clone https://github.com/StanfordASL/pora-exercises.git We denote Problems requiring hand-written solutions and coding in Python withand, respectively. Problem 1: Object Detection Using Pre-trained Models In this exercise, you will get to experiment with pre-trained computer vision models for image object detection. Using the provided notebook ch10/exercises/object _ detection.ipynb: 1. Implement the code to load and evaluate a pre-trained model for object detec- tion. 2. Implement the function draw _ result to create an image with the bounding boxes, labels, and scores overlaid. 3. Implement the function filter to filter the boxes, labels, and scores based on a score threshold. object detection and recognition 277 References [23]Nicolas Carion et al. âEnd-to-End Object Detection with Transformersâ. In: Computer Vision â ECCV 2020. Springer International Publishing, 2020, p. 213â229. [40]R. Girshick. âFast R-CNNâ. In: 2015 IEEE International Conference on Com- puter Vision (ICCV). 2015, p. 1440â1448. doi: 10.1109/ICCV.2015.169. [50]K. He et al. âMask R-CNNâ. In: Proceedings of the IEEE International Con- ference on Computer Vision. 2017, p. 2961â2969. [121]J. Redmon et al. âYou Only Look Once: Unified, Real-Time Object Detec- tionâ. In: Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR). June 2016. [124]O. Ronneberger, P. Fischer, and T. Brox. âU-Net: Convolutional Networks for Biomedical Image Segmentationâ. In: CoRR abs/1505.04597 (2015). arXiv: 1505.04597. url: http://arxiv.org/abs/1505.04597. [129]S. Shi, X. Wang, and H. Li. âPointRCNN: 3D Object Proposal Generation and Detection From Point Cloudâ. In: The IEEE Conference on Computer Vision and Pattern Recognition (CVPR). June 2019. [149]X. Tian et al. âOcc3D: A Large-Scale 3D Occupancy Prediction Benchmark for Autonomous Drivingâ. In: arXiv preprint arXiv:2304.14365 (2023). [153]J. R. R. Uijlings et al. âSelective Search for Object Recognitionâ. In: Inter- national Journal of Computer Vision (2013). [167]Y. Zhou and O. Tuzel. âVoxelnet: End-to-end learning for point cloud based 3d object detectionâ. In: Proceedings of the IEEE conference on computer vision and pattern recognition. 2018, p. 4490â4499. Part I Robot Localization and Mapping This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 11 Introduction to Localization and Filtering We have already discussed the robot motion planning problem and surveyed common algorithms for it, ranging from optimal control to sampling-based meth- ods. All of these approaches implicitly assume access to the robotâs current state, for example for initializing trajectory optimization methods or for closing the loop in feedback control. In practice, however, this state cannot be read directly; it must be estimated from noisy, partial sensor data. Robot perception, as introduced in previous chapters, tackles the challenge of extracting semantic and geometric information from raw sensor streams. These methods are indispensable for local, instantaneous awareness. For instance, de- tecting nearby obstacles with a laser scanner or identifying objects in view with a camera. Yet this information is inherently local and relative to the robotâs cur- rent position. It suffices for collision avoidance, but not for the global reasoning required by full planning and control schemes. This gap is addressed by robot localization and mapping, one of the core compo- nents of the âthinkâ stage in the classical âsee-think-actâ cycle. The goal of robot localization and mapping is to synthesize local sensor data into a coherent global estimate of the robotâs state and map the surrounding environment. In this chap- ter, we focus on localization, which is the ability to infer the robotâs current state with respect to a global frame or map 1 . For instance, before a robot can navigate 1 S. Thrun, W. Burgard, and D. Fox. Probabilistic Robotics.MIT Press, 2005, R. Siegwart, I. R. Nourbakhsh, and D. Scaramuzza. Introduction to Autonomous Mobile Robots. MIT Press, 2011 to a target room on the floor plan shown in Chapter 11, it must first establish where in the building it is located. Figure 11.1: Localization is cru- cial for autonomy:to move from A to B, the robot must know which room it occupies, and that the only path to B runs through the hallway.In- ferring such global information from local range measurements requires specialized algorithms. This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 282 principles of robot autonomy A central challenge in localization is uncertainty. Sensor data is noisy, incom- plete, and sometimes ambiguous. To handle this, localization is cast in a proba- bilistic framework: instead of maintaining a single guess of the robotâs state, we maintain a belief distribution over possible states. This representation allows us to extract both point estimates and measures of uncertainty. Uncertainty quantifica- tion is vital for downstream tasks. For example, a planner may avoid high-risk trajectories under localization uncertainty, or even select actions that deliberately reduce uncertainty through information gathering. The rest of this chapter is organized as follows. In Section 11.1, we review key concepts in probability theory, including random variables, probability distribu- tions, conditional probabilities, and Bayesâ rule. In Section 11.2, we introduce Markov models as a probabilistic representation of robot motion and sensing. Finally, in Section 11.3, we derive the Bayes filter, a recursive algorithm for main- taining and updating a belief distribution as controls are applied and new mea- surements arrive. 11.1 Preliminary Concepts in Probability Tools from probability theory provide us with a way to systematically reason about uncertainty in robotics. Specifically, these tools provide the language to model noisy sensor measurements, uncertain robot states, and stochastic envi- ronments. In this section, we review several key building blocks that form the foundation for probabilistic filtering algorithms such as the Bayes filter, namely random variables, probability distributions, conditional probabilities, and Bayesâ rule. 11.1.1 Random Variables We can model uncertain quantities in robotics, such as sensor measurements, robot states, or environmental properties, as random variables. Depending on the domain of possible values, random variables are classified as discrete or continu- ous. Definition 11.1.1 (Discrete random variable). A discrete random variable X takes values from a countable set. The probability that X takes on a specific value x is denoted by p(X = x), or more compactly p(x). The function p(x) is called the probability mass function (PMF), and it must satisfy: â x p(x) = 1, where the sum is over all possible values of X. Definition 11.1.2 (Continuous random variable). A continuous random variable X takes values in an uncountable set, typically a subset of R n . Its distribution is characterized by a probability density function (PDF) p(x), which satisfies: Z â ââ p(x) dx = 1. introduction to localization and filtering 283 Unlike the discrete case, the probability that a continuous random variable takes on any exact value is zero: P(X = x) = 0. Intuitively, this is because a single point has zero width and probability mass only accumulates over intervals. Consequently, probabilities are defined over regions rather than individual values. For example, the probability that X lies in the interval [a, b] is: P(a†X †b) = Z b a p(x) dx. Example 11.1.1 (Discrete vs. continuous random variables). A coin flip is a dis- crete random variable, X âheads, tails, with a probability mass function: p(heads) = 1 2 ,p(tails) = 1 2 . In robotics, we typically model the robotâs pose as a continuous random variable. For example, a planar robot pose x â SE(2) 2 can take infinitely many values 2 SE(2) is the special Euclidean group in two dimensions.It represents pla- nar rigid-body transformations and con- sists of a position (x, y) and an ori- entation Ξ.For robots operating in three-dimensional space, the pose lies in SE(3), which represents 3D position and orientation. because both its position and orientation vary continuously. Probability distributions. The probability mass function for discrete random vari- ables and probability density function for continuous random variables are often collectively referred to as probability distributions. There are many ways to param- eterize a probability distribution. For discrete variables, the distribution can be specified explicitly by assigning a probability to each possible value. For continu- ous variables, the distribution is often described by a parametric function defined by a small number of parameters. Choosing an appropriate representation is im- portant in robotics, since it determines both how uncertainty is modeled and how efficiently algorithms can reason about it. 11.1.2 Joint Distributions, Independence, and Conditioning Many robotics problems involve more than one uncertain quantity at a time. For example, a robot might simultaneously reason about its pose, the position of an obstacle, and a sensor reading. In such cases, it is useful to describe the probabilities of multiple random variables together using a joint distribution. Definition 11.1.3 (Joint distribution). The joint distribution of two random vari- ables X and Y specifies the probability that both take on specific values simulta- neously. It is denoted by p(X = x, Y = y), or more compactly p(x, y). Independence. Random variables can be related to each other in important ways. For example, the random variables X = âtoday is cloudyâ and Y = âtoday it is rainingâ are correlated: if there are no clouds, it is unlikely to rain. In contrast, two random variables are probabilistically independent if the value of one does not provide any information about the other. 284 principles of robot autonomy Definition 11.1.4 (Probabilistic independence). Two random variables X and Y are probabilistically independent if and only if: p(x, y) = p(x) p(y).(11.1) Example 11.1.2 (Independent sensor measurements). Suppose a robot uses a proximity sensor and a temperature sensor, modeled by random variables X and Y. Let X â close, medium, far and Y â low, med, high, with p(X = close) = 1/3 and p(Y = med) = 1/5. If the two sensors are independent, the joint probability of observing âcloseâ and âmedâ is: p(X = close, Y = med) = 1 3 · 1 5 = 1 15 . Conditional probability. Another key concept is the probability of one random variable given that another has already been observed. Definition 11.1.5 (Conditional probability). The conditional probability of a random variable X taking value x, given that Y took value y, is: p(x | y) : = p(x, y) p(y) .(11.2) Conditional probabilities allow us to update beliefs when new information becomes available. If X and Y are independent, then p(x | y) = p(x), meaning that knowing Y provides no additional information about X. Example 11.1.3 (Sensor conditional probabilities). Building on Example 11.1.2, consider an obstacle detection variable Z â detected, not detected. Assume that the detection probability depends on the proximity sensor value: p(Z = detected| X = close) = 5 6 , p(Z = detected| X = medium) = 1 3 , p(Z = detected| X = far) = 1 5 . If p(X = close) = 1 3 , then the probability that the robot both detects an obstacle and registers âcloseâ is: p(Z = detected, X = close) = p(Z = detected| X = close) p(X = close) = 5 18 . Conditional independence. Finally, independence can also hold given the outcome of another variable. This concept is known as conditional independence and plays a central role in probabilistic modeling. Definition 11.1.6 (Conditional independence). Two random variables X and Y are said to be conditionally independent given a third variable Z if: p(x, y| z) = p(x | z) p(y| z),(11.3) introduction to localization and filtering 285 for all values of x, y, and z. Equivalently: p(x | y, z) = p(x | z).(11.4) We denote conditional independence as: X â„ Y | Z. Intuitively, conditional independence means that once the value of Z is known, learning the value of Y provides no additional information about X. However, if Z is not known, the variables X and Y may still appear correlated. In other words, the variable Z explains the dependence between X and Y. Example 11.1.4 (Conditional independence in robotics). A mobile robot equipped with two wheel encoders produces measurements of traveled distance: one from the left wheel (X) and one from the right wheel (Y). At first glance, these two mea- surements may seem correlated, since the robotâs motion affects both. However, if we condition on the underlying hidden variable Z = âtrue distance traveled,â the two encoder readings are independent: p(x, y| z) = p(x | z) p(y| z). That is, once the actual distance traveled is known, the left and right encoder readings do not provide additional information about each other. This is a typical use of conditional independence in probabilistic sensor models. 11.1.3 Law of Total Probability The law of total probability links marginal, joint, and conditional probabilities. It provides a systematic way to compute the probability of one random variable by accounting for all possible outcomes of another. Definition 11.1.7 (Law of total probability). For discrete random variables X and Y: p(x) = â y p(x, y) = â y p(x | y) p(y). For continuous random variables: p(x) = Z p(x, y) dy = Z p(x | y) p(y) dy. Intuitively, the law of total probability states that to find the probability of X taking value x, we can sum (or integrate) over all possible values of Y, weighting the conditional probability of X given each value of Y by the probability of that value of Y itself. This process is known as marginalization, and p(x) is called the marginal probability of X. Example 11.1.5 (Robot localization via marginalization). Suppose a robotâs po- sition X depends on which hallway Y it is currently in. We can compute the 286 principles of robot autonomy probability of being at a particular location x by considering every possible hall- way y: p(x) = â y p(x | y) p(y). In practice, this means we marginalize over the possible hallways, combining both the likelihood of being in each hallway and the probability of observing x given that hallway. 11.1.4 Bayesâ Rule The joint probability, p(x, y), between two random variables, X and Y, is related to the conditional probabilities, p(x | y) and p(y| x), from the definition of a con- ditional probability in Equation (11.2). Since we can express the joint probability using either conditional probability, we have: p(x, y) = p(x | y) p(y) = p(y| x) p(x). This relationship is commonly referred to as Bayesâ rule 3 . 3 Sometimes also referred to as Bayesâ the- orem. Definition 11.1.8 (Bayesâ rule). For discrete random variables, X and Y, Bayesâ rule states that: p(x | y) = p(y| x) p(x) p(y) .(11.5) Bayesâ rule is useful because it provides a relationship between the âinverseâ conditional probabilities, p(x | y) and p(y | x). This is particularly important for probabilistic inference problems where we need to infer the value of one random variable from another. For example, suppose we have a good initial guess of the probability distribution 4 , p(x), for a random variable, X. Given new information 4 When we have an estimate of the prob- ability distribution p(x) before any new information is used to update it, we will refer to it as the prior probability. about the outcome of a second random variable, Y, that is related to X, we can use Bayesâ rule to update our belief about the probability distribution of X by computing p(x | y) 5 . Bayesâ rule also extends to cases with additional random 5 This new distribution, which we ob- tained by updating the prior distribu- tion p(x) with the new information about Y, is commonly referred to as the posterior probability. variables. For example, with three random variables, X, Y, and Z, Bayesâ rule is: p(x | y, z) = p(y| x, z) p(x | z) p(y| z) . Example 11.1.6 (Bayesâ rule). Consider a scenario where a robot is trying to figure out if it is in room A or room B inside of a building. The robot has an initial guess that the probability it is in room A is p(A) = 3 4 , and the robot has a camera that can be used to improve the estimate. Suppose that a single image, I, is captured and the features extracted from the image are compared to the known room features which gives the conditional probabilities: p(I | A) = 3 4 ,p(I | B) = 1 2 . We can use Bayesâ rule to compute the posterior probability: p(A| I) = p(I | A) p(A) p(I) , introduction to localization and filtering 287 where we use the law of total probability to compute: p(I) = p(I, A) + p(I, B) = p(I | A) p(A) + p(I | B) p(B), and using p(B) = 1â p(A). 11.1.5 Expectation, Variance, and Covariance Probability distributions describe uncertainty in full detail by assigning probabil- ities to every possible outcome of a random variable. In practice, however, we often summarize a distribution using more compact statistics. Some of the most common statistics used to summarize a distribution include the expected value, variance, and covariance. Expectation. The expectation of a random variable is a measure of the central tendency of its distribution. Definition 11.1.9 (Expectation). The expectation 6 of a random variable X is de- 6 Also referred to as the mean or the first moment of a distribution. noted by E [ X ] . For discrete random variables: E [ X ] = â x x p(x), where the sum is over all outcomes of X. For continuous random variables: E [ X ] = Z x p(x) dx. The expected value can be interpreted as the average outcome obtained if the random variable were sampled repeatedly an infinite number of times. The expectation has several useful properties. One particularly important property is linearity, which states that the expectation of a linear transformation of a random variable is equal to the linear transformation of the expectation of the random variable. Formally, for any random variable X and constants a, bâ R, we have: E [ aX + b ] = aE [ X ] + b. This property holds regardless of the distribution of X. For vector-valued random variables X = [X 1 , . . . , X n ] †, the expectation is de- fined component-wise: E [ X ] = ïŁź ïŁŻ ïŁŻ ïŁ° E [ X 1 ] . . . E [ X n ] ïŁč ïŁș ïŁș ïŁ» . Variance. While the expected value describes the center of a distribution, it does not capture how uncertain the variable is. This uncertainty is measured by the variance. Definition 11.1.10 (Variance). The variance of a random variable X is defined as: Var(X) = E h (Xâ E [ X ] ) 2 i . 288 principles of robot autonomy The variance measures the average squared deviation of the variable from its mean. A large variance indicates that the variable can take values far from the mean, while a small variance indicates that the variable is tightly concentrated around the mean. The square root of the variance is called the standard deviation, often denoted by Ï. Covariance. When dealing with multiple random variables, it is often important to understand how their uncertainties are related. This relationship is captured by the covariance. Definition 11.1.11 (Covariance). The covariance between two random variables X and Y is denoted cov(X, Y) and defined as: cov(X, Y) = E h (Xâ E [ X ] )(Yâ E [ Y ] ) †i = E h XY †i â E [ X ] E [ Y ] †. Intuitively, the covariance describes how two random variables vary together. If the covariance is positive, the variables tend to increase or decrease together. If it is negative, one variable tends to increase when the other decreases. If the covariance is zero, the variables are uncorrelated. Example 11.1.7 (Robot motion uncertainty). Suppose X represents the forward displacement of a robot and Y represents its lateral displacement during a sin- gle motion step. If wheel slip increases as the robot moves farther forward, then larger values of X tend to be associated with larger sideways deviations, produc- ing a positive covariance between X and Y. Conversely, if the robotâs mechanical design or control system tends to stabilize lateral motion during forward travel, the covariance between X and Y may be negative. If forward and lateral displacements arise from unrelated sources, the covariance will be close to zero. 11.2 Markov Models In Chapter 1, we modeled robot motion using kinematics and dynamics, obtain- ing a set of first-order differential equations (see Equation (1.1)) that determinis- tically describe how the state x evolves in time given the current state and control input u. In this section, we generalize this view to a probabilistic setting by intro- ducing Markov models, which describe how the state evolves under uncertainty. Markov models are fundamental to robotics, appearing in localization, mapping, planning, and decision-making under uncertainty problems. State, controls, and measurements. As in Chapter 1, the state x â R n collects all variables relevant to the task at hand. In motion planning and control, this typi- cally includes the robotâs physical state (pose, velocity, etc.), while in localization introduction to localization and filtering 289 or higher-level planning it may also include environment variables such as land- mark positions or object features. We work in discrete time, writing x t for the state at time t. We also use the shorthand x t 1 :t n : = x t 1 , x t 2 , . . . , x t n for sequences of states, with analogous notation for control inputs u t 1 :t n and measurements z t 1 :t n . 7 7 Measurements can come from any of the sensors introduced earlier, such as cameras, lidar, or inertial units. Unlike deterministic dynamics, Markov models specify probability distribu- tions over possible states and observations. In full generality, the state evolution is modeled as: p(x t | x 0:tâ1 , z 1:tâ1 , u 1:t ),(11.6) which captures the distribution of the current state x t conditioned on the entire history of past states, controls, and measurements. Following the convention used throughout this chapter, the robot first executes the control u t , then receives the measurement z t based on the resulting state x t . The corresponding proba- bilistic measurement model is: p(z t | x 0:t , z 1:tâ1 , u 1:t ).(11.7) The Markov property. In many applications, we define the state x t to be complete, meaning it contains all the information necessary to predict future states. For- mally, this assumption implies that past states and measurements provide no additional predictive power beyond x tâ1 and u t . This is known as the Markov property, under which the models simplify to: p(x t | x tâ1 , u t ),(11.8) for the state transition, and: p(z t | x t ),(11.9) for the measurement model. Markov models in robotics. A Markov model thus consists of a state transition dis- tribution (11.8) and a measurement distribution (11.9). Intuitively, the transition model captures process uncertainty (e.g., wheel slip when applying a control), while the measurement model captures sensor noise (e.g., rangefinder variabil- ity). Together, these components form the foundation of probabilistic state esti- mation (Section 11.2). The probabilistic model described above belongs to a broader family of models that assume the system state evolves according to the Markov property. Several closely related formulations exist, depending on whether control inputs and ob- servability are considered. In robotics, the state of the system is typically not directly observable. Instead, sensors provide indirect and noisy measurements that depend on the underlying state. Models with this structure are often referred to as partially observable Markov models. If control inputs are not present, the model reduces to a hidden Markov model (HMM). In this case, the state evolves according to a Markov process, and obser- vations provide partial information about that state. The term âhiddenâ reflects 290 principles of robot autonomy x tâ1 x t x t+1 u tâ1 u t u t+1 z tâ1 z t z t+1 Figure 11.2: Graphical represen- tation of a Markov model. At each time step, the control u t in- fluences the new state x t , and the resulting state generates a measurement z t . the fact that the true state x t cannot be observed directly and must instead be inferred from the sequence of measurements. 11.3 Bayes Filter Robot localization is a classic instance of a filtering problem 8 where our goal is 8 Localization is one instance of the more general filtering problem referred to as state estimation. to compute a probability distribution over the current state x t given the history of control inputs u 1:t , and measurements z 1:t . One of the canonical approaches to this filtering problem is known as the Bayes filter or recursive Bayesian estimation. The Bayes filter leverages a Markov model to recursively update a belief distribu- tion, which is a probability distribution over x t . Mathematically, we denote the belief distribution as bel(x t ) and define it as: bel(x t ) : = p(x t | z 1:t , u 1:t ).(11.10) In other words, the belief bel(x t ) is a posterior probability distribution over the state conditioned on the available history information. We also define a distribu- tion called the prediction distribution as: bel(x t ) : = p(x t | z 1:tâ1 , u 1:t ),(11.11) which does not include the most recent measurement z t . We call the process of using the new measurement z t to compute the belief bel(x t ) from the predicted belief bel(x t ) a correction or measurement update. The Bayes filter consists of a pre- diction step for computing bel(x t ) from the prior belief followed by a correction step for computing bel(x t ) given the new measurement z t . 11.3.1 Algorithm The recursive structure of the Bayes filter is summarized in Algorithm 11.1. At each time step, the filter maintains a belief distribution belief bel(x t ) over the system state. The inputs to the filter are the previous belief 9 , the current control 9 In practice, we initialize the prior distri- bution bel(x 0 ) using either a best guess of the initial state or a uniform distribu- tion when no prior information is avail- able. input u t , and the latest sensor measurement z t . The algorithm proceeds in two stages. First, a prediction step computes the distribution bel(x t ) over possible states after applying the control input. This introduction to localization and filtering 291 step propagates uncertainty forward using the state transition model from Equa- tion (11.8). Second, a correction step incorporates the new measurement using the measurement model from Equation (11.9), adjusting the predicted belief to better match the observed data. Intuitively, the prediction step estimates where the robot might be after execut- ing the control, while the correction step refines this estimate using information from the sensors. Algorithm 11.1: Bayes Filter Data: bel(x tâ1 ), u t , z t Result: bel(x t ) foreach x t do // Prediction (motion update) bel(x t ) = R p(x t | x tâ1 , u t ) bel(x tâ1 ) dx tâ1 // Correction (measurement update) bel(x t ) = η p(z t | x t ) bel(x t ) return bel(x t ) In Algorithm 11.1, η is a normalization constant ensuring bel(x t ) integrates (or sums) to one. 10 Conceptually, the Bayes filter performs a repeated predictâcorrect 10 In practice, η = 1/ p(z t | z 1:tâ1 , u 1:t ), which follows directly from Bayesâ rule. cycle, where the motion model spreads the belief forward to account for process uncertainty, while the measurement model reshapes the belief according to how consistent each state is with the observed sensor measurement. 11.3.2 Derivation We now derive the Bayes filter recursion from the definition of the belief distribu- tion introduced in Section 11.3. Applying Bayesâ rule to Equation (11.10) yields: bel(x t ) = p(x t | z 1:t , u 1:t ) = η p(z t | x t , z 1:tâ1 , u 1:t ) p(x t | z 1:tâ1 , u 1:t ), (11.12) where η is a normalization constant: η = 1 p(z t | z 1:tâ1 , u 1:t ) . Measurement update. Using the conditional independence assumptions of the Markov model (Section 11.2), the current measurement depends only on the cur- rent state. Therefore: p(z t | x t , z 1:tâ1 , u 1:t ) = p(z t | x t ). Substituting this simplification into Equation (11.12) gives: bel(x t ) = η p(z t | x t ) p(x t | z 1:tâ1 , u 1:t ). 292 principles of robot autonomy Recall the definition of the prediction belief from Equation (11.11): bel(x t ) : = p(x t | z 1:tâ1 , u 1:t ), we obtain: bel(x t ) = η p(z t | x t ) bel(x t ), which corresponds to the measurement update step of the Bayes filter. Prediction update. Next we derive an expression for the prediction belief bel(x t ). Starting from its definition in Equation (11.11), we apply the law of total proba- bility to marginalize over the previous state x tâ1 : bel(x t ) = Z p(x t , x tâ1 | z 1:tâ1 , u 1:t )dx tâ1 = Z p(x t | x tâ1 , z 1:tâ1 , u 1:t ) p(x tâ1 | z 1:tâ1 , u 1:t )dx tâ1 . Using the Markov assumption again, the next state depends only on the pre- vious state and the current control input: p(x t | x tâ1 , z 1:tâ1 , u 1:t ) = p(x t | x tâ1 , u t ). Furthermore, the control input u t does not influence the previous state x tâ1 , so: p(x tâ1 | z 1:tâ1 , u 1:t ) = p(x tâ1 | z 1:tâ1 , u 1:tâ1 ). Recognizing that, by definition: bel(x tâ1 ) = p(x tâ1 | z 1:tâ1 , u 1:tâ1 ), we obtain the prediction step: bel(x t ) = Z p(x t | x tâ1 , u t ) bel(x tâ1 ) dx tâ1 . 11.3.3 Discrete Bayes Filter When the state space is finite, the belief distribution can be represented as a probability mass function over a discrete set of statesx k . In this case, the belief at time t is described by a collection of probabilities p k,t , where p k,t denotes the probability that the system is in state x k at time t. The Bayes filter recursion can then be written in discrete form by replacing the integrals in Algorithm 11.1 with summations over the possible states. The resulting algorithm, shown in Algorithm 11.2, follows the same two-step procedure as the continuous Bayes filter, with a prediction step using the transition model, followed by a correction step using the measurement model. Here p k,t denotes the predicted probability of state x k before incorporating the measurement, and η is a normalization constant ensuring that the probabilities sum to one. introduction to localization and filtering 293 Algorithm 11.2: Discrete Bayes Filter Data: p k,tâ1 , u t , z t Result: p k,t foreach k do p k,t = â i p(x t | x i , u t ) p i,tâ1 p k,t = η p(z t | x k ) p k,t return p k,t 11.3.4 Practical Considerations The Bayes filter provides a general and principled framework for probabilistic state estimation. However, applying it directly is often computationally challeng- ing in realistic robotics problems. In continuous state spaces, the prediction step requires evaluating integrals over the entire state space. Computing these integrals exactly is often intractable, resulting in the need for approximations or numerical methods to estimate the belief distribution. In discrete state spaces, the recursion can be computed exactly, but the required summations scale with the number of possible states. As the dimensionality of the state increases, the number of discrete states grows rapidly, making exact inference computationally expensive. Despite these challenges, the Bayes filter serves as the conceptual foundation for many practical state estimation algorithms. Widely used filters such as the Kalman filter, extended Kalman filter (EKF), unscented Kalman filter (UKF), and particle filter can all be viewed as specific implementations of the Bayes filter that exploit additional assumptions or approximations to make the computation tractable. Example 11.3.1 (Robot in a hallway). Consider a robot moving along a straight hallway represented by a one-dimensional line. The hidden state x t is the robotâs position along this line. At each time step t the robot receives a control input u t representing a commanded forward displacement and a measurement z t repre- senting the noisy distance to the nearest door in front of the robot, obtained from a range sensor. Prediction. Suppose the robot starts at position x tâ1 = 2 m with a belief concen- trated around that location and issues a command u t = +0.5 m forward. Due to wheel slip and actuator noise, the true displacement may vary, which we model as: âx âŒN (u t , 0.1 2 ). The prediction step therefore spreads the belief forward, producing belief bel(x t ) centered at 2.5 m but with larger variance than the previous belief. 294 principles of robot autonomy Correction. At the same step, the robotâs range sensor reports z t = 2.4 m to the next door. The sensor is noisy and modeled by: p(z t | x t ) =N (z t ; true distance(x t ), 0.05 2 ). If the predicted belief assigns significant probability to states near x t â 2.5 m and the hallway map indicates that the next door lies roughly 2.4 m ahead from that position, the measurement is consistent with the prediction. The correction step therefore increases the probability of these states and sharpens the belief distribution around them. States that would predict very different measurements are downweighted. Recursive operation. Over time, the filter alternates prediction and correction. The prediction step tends to broaden the belief because motion introduces un- certainty, while the correction step can concentrate the belief when informative measurements are received. In hallways with repeating structures, several posi- tions may initially produce similar sensor readings, causing the belief to remain multimodal. As the robot gathers additional observations, the Bayes filter resolves this ambiguity and the belief collapses around the robotâs true location. In the repository github.com/StanfordASL/pora-exercises, the notebook ch11/discrete _ bayes.ipynb provides a concrete implementation of the discrete Bayes filter for this hallway example. Running the notebook illustrates how the belief distribution evolves over time as the robot moves and collects measure- ments. 11.4 Summary In this chapter, we introduced the probabilistic foundations underlying robot lo- calization and state estimation. We began by motivating why localization is es- sential for autonomy: robots must infer their pose from noisy and partial sensor data rather than directly observing it. To reason systematically about this un- certainty, we reviewed key concepts in probability theory, including random vari- ables, probability distributions, conditional independence, and Bayesâ rule. These concepts provide the mathematical framework for representing and updating un- certainty in robotic systems. Building on these foundations, we introduced Markov models as probabilis- tic representations of robot motion and sensing. These models define how the robotâs state evolves over time and how measurements relate to that state, form- ing the basis for probabilistic inference in dynamic systems. Finally, we derived the Bayes filter, a recursive algorithm for estimating a belief distribution over the robotâs state as it moves and collects new measurements. The Bayes filter provides the conceptual foundation for many practical estimation algorithms. Widely used methods such as the Kalman filter, extended Kalman filter, unscented Kalman fil- ter, and particle filter can all be understood as specific implementations of this general framework. In the following chapters, we build on these ideas to develop introduction to localization and filtering 295 practical algorithms for localization, mapping, and simultaneous localization and mapping. To learn more. For a comprehensive introduction to probabilistic robotics, includ- ing localization and filtering, readers are referred to the seminal text by Thrun, Burgard, and Fox [148], which provides an intuitive and rigorous treatment of the Bayes filter and its extensions. Additional foundational perspectives on prob- abilistic reasoning and estimation in robotics can be found in Siegwart, Nour- bakhsh, and Scaramuzza [135] and Maybeck [93]. For readers interested in a more theoretical background in stochastic systems and control, Gelb et al. [39] of- fers a classical reference on estimation theory. Finally, Carlone et al. [24] presents a modern, unified treatment of localization, filtering, and mapping under a com- mon probabilistic framework. 11.5 Exercises The starter code for the exercises provided below is available online through GitHub. To get started, download the code by running in a terminal window: git clone https://github.com/StanfordASL/pora-exercises.git We denote Problems requiring hand-written solutions and coding in Python withand, respectively. Problem 1: Airport Security Suppose that travelers passing through an airport carry prohibited items 1% of the time. Each passenger passes through a simple detector that has a true positive detection rate of 92% and false positive detection rate of 8%. Passengers that trigger the detector alarm are sent to a more accurate secondary screening that has a true positive detection rate of 98% and a false positive detection rate of 10%. 1. Given that a randomly selected passenger was flagged by the second screening, what is the probability a passenger was carrying a prohibited item? 2. How does the probability change in this case if all passengers were to get the more accurate secondary screening and the first simple detector was not used? Problem 2: Cookie Machine You operate a cookie-making machine that when activated will produce a random number of cookies in the range [1, . . . , N], with each quantity equally likely with probability 1 N . Each cookie costs $1 to produce. 296 principles of robot autonomy 1. Each of your customers will pay a fixed price to activate the machine. What is the minimum fixed price you should charge each customer to ensure that you donât lose money in the long run? In other words, what is the expected cost of each customer activation? Suppose the machine manufacturer produces machines with N = 6 and N = 10, and unfortunately they donât know which one they sent you, but the chance you received either machine is equally likely. However, they did record that in a test run of your machine it produced 5 cookies. 2. What is the probability that you received a machine configured with N = 6? 3. What is the expected number of cookies that will be produced the first time you operate the machine? introduction to localization and filtering 297 References [24]L. Carlone et al., eds. SLAM Handbook. From Localization and Mapping to Spatial Intelligence. Cambridge University Press, 2026. [39]A. Gelb et al. Applied optimal estimation. MIT press, 1974. [93]P. S. Maybeck. Stochastic models, estimation, and control. Vol. 3. Academic press, 1982. [135]R. Siegwart, I. R. Nourbakhsh, and D. Scaramuzza. Introduction to Au- tonomous Mobile Robots. MIT Press, 2011. [148]S. Thrun, W. Burgard, and D. Fox. Probabilistic Robotics. MIT Press, 2005. 12 Approximate Filters for State Estimation In Chapter 11, we introduced the Bayes filter as the canonical framework for state estimation. The Bayes filter uses a recursive procedure that alternates be- tween prediction, using a probabilistic state transition model, and correction, using a probabilistic measurement model. Intuitively, the prediction step answers the question, âWhere do we expect to be now, given where we were and how we moved?â, while the correction step asks, âHow should we revise that expectation in light of the new sensor data?â While conceptually elegant, the Bayes filter is rarely tractable to implement in its full generality. The integrals in the prediction step and the normalization in the correction step can be computationally intractable for continuous, high- dimensional state spaces. In discrete domains, exact enumeration is possible in principle but becomes impractical as the number of states grows. As a result, practical state estimation algorithms rely on approximations of the belief distri- bution. Over time, two broad families of approximations have emerged: âą Parametric filters: parametric filters assume that the belief distribution belongs to a specific parametric family (most commonly Gaussian), characterized by a fixed set of parameters such as mean and covariance [148]. By exploiting the structure of this representation, the belief can be updated efficiently at each time step. The Kalman filter and its variants, including the extended Kalman filter and unscented Kalman filter, are prominent examples. âą Non-parametric filters: non-parametric filters do not assume a fixed functional form for the belief distribution. Instead, the distribution is approximated di- rectly, either through discretization as in histogram filters or through sampling as in particle filters. This flexibility allows non-parametric methods to repre- sent multimodal and highly irregular belief distributions, although often at a higher computational cost. Viewed together, parametric and non-parametric filters represent two ends of a spectrum. Parametric filters trade representational flexibility for computa- tional efficiency, while non-parametric filters trade efficiency for expressiveness. In robotics practice, both families play a critical role. Parametric filters often suf- This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 300 principles of robot autonomy fice when the problem structure is close to Gaussian and unimodal, while non- parametric filters are indispensable when ambiguity, multimodality, or strong nonlinearities are present. It is helpful to view them as complementary rather than mutually exclusive. Many systems use non-parametric methods for global reasoning, then switch to parametric filters for fast local tracking once a unique hypothesis has been identified. For instance, imagine a mobile robot navigating a building. Early in the mission, it may be unsure which corridor or even which floor it is on. In this case, its belief is naturally multimodal and non-parametric methods shine. As the robot gathers more information and locks onto a unique hypothesis, its uncertainty becomes locally well-approximated by a single Gaus- sian, and parametric filters become attractive for their speed and simplicity. In the remainder of this chapter, we develop both approaches within a uni- fied narrative. We begin by reviewing the Gaussian distribution in Section 12.1, which forms the foundation of parametric filters. We then introduce the Kalman filter and its variants in Section 12.2 and Section 12.3, which are the most widely used parametric filters in robotics. Next, we turn to non-parametric filters in Sec- tion 12.4, where we relax these assumptions and represent beliefs more directly through discretization (Section 12.4.1) or sampling (Section 12.4.2). 12.1 The Gaussian Distribution Before we introduce specific filters, it is worth pausing to review the Gaussian dis- tribution, which is the workhorse of parametric state estimation. The Gaussian distribution 1 is one of the most widely used probability distributions in science 1 Also referred to as the Normal distri- bution. and engineering and plays a central role in robotics state estimation. Its impor- tance arises not only from its frequent appearance in natural noise processes, but also from its favorable mathematical properties that make recursive filtering tractable. Informally, in one dimension, a Gaussian distribution resembles the familiar âbell curveâ: it is centered at its mean, ÎŒ, and its spread is controlled by its variance, Ï 2 . In higher dimensions, a Gaussian describes an ellipsoidal cloud of probability mass in the state space. It is high near the mean, low far away, and its covariance matrix tells us in which directions the uncertainty is large or small. Univariate case. The probability density function of a one-dimensional 2 Gaussian 2 We refer to a one-dimensional Gaussian as univariate and to higher-dimensional cases as multivariate. random variable X with mean ÎŒ and variance Ï 2 is: p(x) = 1 â 2ÏÏ 2 exp â 1 2 (xâÎŒ) 2 Ï 2 .(12.1) We write this compactly as X ⌠N (ÎŒ, Ï 2 ), and say that âX is distributed as a Gaussian with mean ÎŒ and variance Ï 2 â. The mean, ÎŒ indicates the center of mass of the distribution, and the variance, Ï 2 , measures how spread out the distribution is around that center. approximate filters for state estimation 301 Multivariate case. For an n-dimensional random vector X â R n with mean ÎŒ â R n and covariance matrixÎŁ â R nĂn , the multivariate Gaussian distribution is defined by: p(x) = 1 p det(2ÏÎŁ) exp â 1 2 (xâ ÎŒ) †Σ â1 (xâ ÎŒ) ,(12.2) and we compactly write X âŒN (ÎŒ,ÎŁ). The covarianceÎŁ captures both the spread of each component of x and their pairwise correlations. Geometrically, the level sets of a multivariate Gaussian 3 are ellipsoids centered at ÎŒ, with shape and 3 Each level set contains points of equal probability density. orientation determined by the covariance matrixÎŁ. The Gaussian distribution exhibits several important mathematical properties related to affine transformations, addition, and multiplication, which make it particularly attractive for use in filtering algorithms. We highlight three that will be used repeatedly in what follows. Affine transformations. The first useful property of the Gaussian distribution is that an affine transformation of a Gaussian random variable is also a Gaussian random variable. If the random vector X has a multivariate Gaussian distribution with mean ÎŒ and covarianceÎŁ, then the random variable Y computed from an affine transformation: Y = AX + b, also has a multivariate Gaussian distribution with mean AÎŒ + b and covariance AÎŁ A †. In other words, if X âŒN (ÎŒ,ÎŁ), then Y âŒN (AÎŒ + b, AÎŁ A †). In the context of robotics, this tells us that if our belief over the current state is Gaussian and the dynamics are linear with additive Gaussian noise, then the predicted state is also Gaussian. We can therefore keep track of just the mean and covariance instead of an entire arbitrary density. Sum. The next useful property of Gaussians is that the sum of two indepen- dent Gaussian random variables is also a Gaussian random variable. Suppose X 1 and X 2 have multivariate Gaussian distributions with means ÎŒ 1 and ÎŒ 2 and co- variancesÎŁ 1 andÎŁ 2 . Then, the random variable Y computed by the sum: Y = X 1 + X 2 , also has a multivariate Gaussian distribution with mean ÎŒ 1 + ÎŒ 2 and covari- anceÎŁ 1 +ÎŁ 2 . In other words, if X 1 ⌠N (ÎŒ 1 ,ÎŁ 1 ) and X 2 ⌠N (ÎŒ 2 ,ÎŁ 2 ), then Y âŒN (ÎŒ 1 + ÎŒ 2 ,ÎŁ 1 +ÎŁ 2 ). In robotics, this property commonly appears when modeling additive noise. For example, if a robotâs predicted state is Gaussian and we add independent Gaussian process noise, the resulting state distribution remains Gaussian with covariance equal to the sum of the individual covariances. 302 principles of robot autonomy Product. The product of two Gaussian probability density functions is also a Gaussian probability density function. Consider two Gaussian probability den- sity functions: p 1 (x) = 1 p det(2ÏÎŁ 1 ) exp â 1 2 (xâ ÎŒ 1 ) †Σ â1 1 (xâ ÎŒ 1 ) p 2 (x) = 1 p det(2ÏÎŁ 2 ) exp â 1 2 (xâ ÎŒ 2 ) †Σ â1 2 (xâ ÎŒ 2 ) . Their product is: p(x) = p 1 (x)· p 2 (x) = exp â 1 2 (xâ ÎŒ 1 ) †Σ â1 1 (xâ ÎŒ 1 )â 1 2 (xâ ÎŒ 2 ) †Σ â1 2 (xâ ÎŒ 2 ) (2Ï) d p det(ÎŁ 1 ) p det(ÎŁ 2 ) = exp â 1 2 (xâ ÎŒ) †Σ â1 (xâ ÎŒ) exp â 1 2 (ÎŒ †1 ÎŁ â1 1 ÎŒ 1 + ÎŒ †2 ÎŁ â1 2 ÎŒ 2 â ÎŒ †Σ â1 ÎŒ) (2Ï) d p det(ÎŁ 1 ) p det(ÎŁ 2 ) , where d is the dimension of the covariance matrices, and we can see that the second exponential is constant with respect to x. Therefore, the product is a Gaussian probability density function with mean ÎŒ and covarianceÎŁ: ÎŁ = (ÎŁ â1 1 +ÎŁ â1 2 ) â1 , ÎŒ =ÎŁ(ÎŁ â1 1 ÎŒ 1 +ÎŁ â1 2 ÎŒ 2 ). This property underlies the Bayes filter measurement update, where multiply- ing a Gaussian prior by a Gaussian likelihood yields a Gaussian posterior. The update simply shifts the mean and shrinks or expands the covariance according to how informative and reliable the measurement is. Why Gaussians in filtering? These properties ensure that when both the transition and measurement models are linear with Gaussian noise, the Bayes filter reduces to simple recursive updates of the mean and covariance. This leads directly to the family of Kalman filters, which we will introduce in the next section. From a computational point of view, this is extremely attractive. Instead of carrying around an entire function p(x t ), we only need to carry a vector ÎŒ t and a matrixÎŁ t and update them at each time step. 12.2 Kalman Filter The Kalman filter is the canonical parametric realization of the Bayes filter for systems with linear dynamics and Gaussian noise. Specifically, the Kalman filter uses a multivariate Gaussian distribution to parameterize the belief distribution over possible states. In other words, we assume x t âŒN (ÎŒ t ,ÎŁ t ), so that: bel(x t ) = 1 p det(2ÏÎŁ t ) exp â 1 2 (x t â ÎŒ t ) †Σ â1 t (x t â ÎŒ t ) . approximate filters for state estimation 303 Example 12.2.1 (Constant-velocity motion in one dimension). Consider a robot moving along a straight corridor. The state encodes position and velocity: x t = " p t v t # , and the robot receives noisy position measurements from a range sensor. A com- mon linear-Gaussian model is: x t = A t x tâ1 + B t u t + Δ t , z t = C t x t + ÎŽ t , where: A t = " 1ât 01 # ,B t = " 1 2 ât 2 ât # ,C t = h 1 0 i , and where the random variables Δ t ⌠N (0, Q t ), ÎŽ t ⌠N (0, R t ) model process noise and measurement noise, respectively. In this setting, the Kalman filter pro- vides the optimal recursive estimator of x t in the mean-squared error sense. Like the Bayes filter, the Kalman filter is split up into two steps, a prediction step and measurement update step. Both steps update the mean ÎŒ and covari- anceÎŁ of the Gaussian belief and rely on several structural assumptions about the system. We first assume that the initial belief bel(x 0 ) is Gaussian with x 0 âŒN (ÎŒ 0 ,ÎŁ 0 ). We also assume that the state transition model is linear and evolves according to: x t = A t x tâ1 + B t u t + Δ t ,(12.3) where x tâ1 denotes the previous state, u t is the current control input, Δ t is an in- dependent process noise that is normally distributed according to Δ t âŒN (0, Q t ), and A t and B t are time-varying matrices that define the dynamics. The ma- trix Q t â R nĂn is the process noise covariance and captures uncertainty in the motion model 4 4 Examples include unmodeled accelera- tions and environmental disturbances. The affine structure of the dynamics together with Gaussian noise ensures that the distribution of the next state remains Gaussian. That is, if x tâ1 is Gaussian, then x t is also Gaussian. The corresponding probabilistic transition model can therefore be written as: p(x t | x tâ1 , u t ) = 1 p det(2ÏQ t ) exp â 1 2 (x t â A t x tâ1 â B t u t ) †Q â1 t (x t â A t x tâ1 â B t u t ) , and therefore the next state is normally distributed with: x t âŒN (A t x tâ1 + B t u t , Q t ). We also assume that the measurement model is linear and of the form: z t = C t x t + ÎŽ t ,(12.4) 304 principles of robot autonomy where ÎŽ t is an independent measurement noise that is normally distributed ac- cording to ÎŽ t ⌠N (0, R t ), and C t is a time-varying matrix that defines how the state maps to measurements. The matrix R t â R mĂm is the measurement noise covariance that describes the uncertainty in the sensor measurements. Under these assumptions, the probabilistic measurement model can be ex- pressed as: p(z t | x t ) = 1 p det(2ÏR t ) exp â 1 2 (z t â C t x t ) †R â1 t (z t â C t x t ) , which implies that z t | x t ⌠N (C t x t , R t ). In many robotics applications, the ma- trix C t selects a subset of the state variables. For example a sensor may measure position but not velocity. To summarize, the Kalman filter assumes that the initial belief is Gaussian and that both the state transition model and measurement model are linear with additive Gaussian noise. These assumptions guarantee that the belief distribution remains Gaussian after each prediction and measurement update. As a result, the algorithm only needs to propagate the mean ÎŒ and covarianceÎŁ rather than a full probability distribution. Therefore, while this property makes the Kalman filter computationally efficient, it also limits its applicability to systems that satisfy the assumptions of linearity and Gaussian noise. 12.2.1 Algorithm (PredictâCorrect Form) For clarity, we distinguish between the predicted belief, obtained after applying the control but before incorporating the new measurement, and the corrected belief, obtained after the measurement update. Algorithm 12.1 details the full algorithm, where we denote the predicted mean and covariance by Ì ÎŒ t and Ì ÎŁ t . Intuition. The Kalman filter can be understood as a repeated negotiation be- tween the model prediction and the sensor measurement. The prediction step propagates the belief through the system dynamics while adding process uncer- tainty. The measurement update then adjusts this prediction using the newly observed measurement, where the amount of adjustment depends on how re- liable the model prediction and the sensor reading are relative to one another. Specifically, the residual Ìz t measures the discrepancy between the predicted mea- surement and the actual observation. The matrix S t describes the expected uncer- tainty of this residual and is known as the innovation covariance. The Kalman gain K t determines how strongly the estimate should respond to the measurement: when sensor noise is large, K t becomes small and the filter relies more heavily on the prediction; when the model uncertainty is large but the sensor is precise, K t increases and the estimate is pulled more strongly toward the measurement. approximate filters for state estimation 305 Algorithm 12.1: Kalman Filter (linear-Gaussian) Data: ÎŒ tâ1 ,ÎŁ tâ1 , u t , z t Result: ÎŒ t ,ÎŁ t // Prediction (motion update) ÌÎŒ t â A t ÎŒ tâ1 + B t u t Ì ÎŁ t â A t ÎŁ tâ1 A †t + Q t // Innovation (measurement residual) Ìz t â z t â C t ÌÎŒ t S t â C t Ì ÎŁ t C †t + R t // Kalman gain K t â Ì ÎŁ t C †t S â1 t // Correction (measurement update) ÎŒ t â ÌÎŒ t + K t Ìz t // Covariance update (Joseph form for numerical stability) ÎŁ t â (Iâ K t C t ) Ì ÎŁ t (Iâ K t C t ) †+ K t R t K †t return ÎŒ t ,ÎŁ t 12.2.2 Derivation One way to derive the Kalman filter algorithm is by explicitly evaluating the Bayes filter updates from Chapter 11 with the Gaussian belief structure and prob- abilistic transition and measurement models. This would involve explicitly com- puting an integral of p(x t | x tâ1 , u t ) p(x tâ1 ) for the prediction step. Instead, we consider a more intuitive approach that directly leverages the properties of Gaussians presented in Section 12.1 to show that the familiar update equations in Algorithm 12.1 are an exact consequence of the linear-Gaussian assumptions. First, from the prior belief distribution, bel(x tâ1 ) ⌠N (ÎŒ tâ1 ,ÎŁ tâ1 ), we com- pute the predicted belief, bel(x tâ1 ), by using the affine transformation property of Gaussian random variables and the property concerning the sum of two in- dependent Gaussian random variables. Specifically, we apply these properties to the linear state transition model in Equation (12.3) to give the predicted mean: ÌÎŒ t = A t ÎŒ tâ1 + B t u t + 0, where the 0 comes from the mean of the independent Gaussian process noise, Δ t ⌠N (0, Q t ). The predicted covariance is then: Ì ÎŁ t = A t ÎŁ tâ1 A †t + Q t . For the measurement update step of the Bayes filter, we have: bel(x t )â p(z t | x t ) bel(x t ), where p(z t | x t ) ⌠N (C t x t , R t ) and bel(x t ) ⌠N ( ÌÎŒ t , Ì ÎŁ t ). We can therefore use the fact that the product of two Gaussians probability density functions is also a 306 principles of robot autonomy Gaussian probability density function to compute: bel(x t ) = η exp â 1 2 J t , where η is a normalization constant and: J t = (z t â C t x t ) †R â1 t (z t â C t x t ) + (x t â ÌÎŒ t ) â€ Ì ÎŁ â1 t (x t â ÌÎŒ t ). We compute the mean, ÎŒ t , for this new probability density function by finding where the first derivative of bel(x t ) with respect to x t is zero, which occurs when the derivative of J t with respect to x t is zero. Similarly, we compute the new covariance,ÎŁ t , as the inverse of the second derivative of J t with respect to x t . Therefore, we have the conditions: 0 =âC †t R â1 t (z t â C t ÎŒ t ) + Ì ÎŁ â1 t (ÎŒ t â ÌÎŒ t ), ÎŁ â1 t = C †t R â1 t C t + Ì ÎŁ â1 t , which give: ÎŁ t = (C †t R â1 t C t + Ì ÎŁ â1 t ) â1 . Through algebraic manipulation, we write the mean in terms of the covarianceÎŁ t : ÎŒ t = ÌÎŒ t +ÎŁ t C †t R â1 t (z t â C t ÌÎŒ t ). With a few additional algebraic steps, we now transform these equations in the form of the Kalman filter equations in Algorithm 12.1. From the matrix inversion lemma, we write: (C †t R â1 t C t + Ì ÎŁ â1 t ) â1 = Ì ÎŁ t â Ì ÎŁ t C †t (C t Ì ÎŁ t C †t + R t ) â1 C t Ì ÎŁ t , and then we define the Kalman gain as K t : = Ì ÎŁ t C †t (C t Ì ÎŁ t C †t + R t ) â1 so that the covariance is given by: ÎŁ t = Ì ÎŁ t â K t C t Ì ÎŁ t . Through some additional algebraic manipulations, we express the mean in terms of the Kalman gain to get: ÎŒ t = ÌÎŒ t + K t (z t â C t ÌÎŒ t ). Further details on this derivation and the algebraic steps involved can be found in Thrun, Burgard, and Fox [148]. 12.2.3 Practical Considerations The Kalman filter exploits the structure of the Gaussian distribution, which makes it a computationally efficient algorithm for filtering in a continuous state space. approximate filters for state estimation 307 However, the use of Gaussian beliefs also restricts the flexibility of the proba- bilistic model, since we have to assume the sufficiency of linear state transition and measurement models. In practice, this linearity assumption may not be very accurate with respect to the real world behavior of the robot and sensors. The structure also limits the belief distribution to be unimodal, which may limit per- formance in some applications 5 . 5 For example, in robot localization tasks, a multimodal distribution can better capture the global distribution. Despite these limitations, Kalman filters are ubiquitous in robotics. They are widely used for fusing inertial sensors, tracking moving objects, estimating ve- locities from position-only measurements, and many other tasks where the state is reasonably well modeled as evolving linearly with Gaussian noise. In the next section, we introduce extensions to the Kalman filter that relax the linearity as- sumptions while retaining the Gaussian belief structure, which further expands the applicability of this family of filters to a wider range of robotics problems. 12.3 Kalman Filter Extensions The Kalman filter provides an exact recursive solution to the Bayes filter when the dynamics and measurement models are linear and all uncertainty is Gaussian. In robotics, however, these assumptions are often only approximately true, since robot motion and sensing commonly involve nonlinear relationships. To address this limitation, several extensions have been developed that preserve the Gaussian belief representation while accommodating nonlinear models. In this section, we introduce two of the most widely used approaches: the ex- tended Kalman filter (EKF) and the unscented Kalman filter (UKF). The EKF handles nonlinearities by linearizing the dynamics and measurement models around the current belief mean, while the UKF propagates a carefully chosen set of sam- ple points through the nonlinear models to capture how the belief distribution transforms. Together, these methods provide practical and widely used tools for nonlinear state estimation when a unimodal Gaussian approximation remains appropriate. 12.3.1 Extended Kalman Filter (EKF) The EKF generalizes the Kalman filter to nonlinear process and measurement models while retaining a Gaussian belief parameterization. It does so by locally linearizing the nonlinear models and then applying Kalman-style predictâcorrect updates to the mean and covariance. This makes the EKF a practical default for many robotics state estimation tasks with smooth nonlinear dynamics and sensing. Instead of the linear models in Equation (12.3) and Equation (12.4) used by the Kalman filter, the EKF considers general nonlinear state transition and measure- ment models of the form: x t = f (x tâ1 , u t ) + Δ t , z t = h(x t ) + ÎŽ t , (12.5) 308 principles of robot autonomy where Δ t ⌠N (0, Q t ) and ÎŽ t ⌠N (0, R t ) are normally distributed process and measurement noise terms. The EKF incorporates these nonlinear models into the prediction and mea- surement update steps of the filter in two ways. First, by evaluating the nonlinear models directly, and second, by linearizing them via a first-order Taylor series ex- pansion. We perform the first-order Taylor series expansion of the state transition model, f (x tâ1 , u t ), about the most likely state from the current belief distribution, which is the expected value, ÎŒ tâ1 : f (x tâ1 , u t )â f (ÎŒ tâ1 , u t ) + F t (x tâ1 â ÎŒ tâ1 ), where F t = â x f (ÎŒ tâ1 , u t ) is the Jacobian of f (x tâ1 , u t ) evaluated at ÎŒ tâ1 . Using this linear approximation, we write the probabilistic state transition model as: p(x t | x tâ1 , u t ) = 1 p det(2ÏQ t ) exp â 1 2 âx †t Q â1 t âx t , where: âx t = x t â f (ÎŒ tâ1 , u t )â F t (x tâ1 â ÎŒ tâ1 ). The prediction step of the EKF leverages the nonlinear state transition model and the linearized model to update the mean and covariance as: ÌÎŒ t = f (ÎŒ tâ1 , u t ), Ì ÎŁ t = F t ÎŁ tâ1 F †t + Q t , which exhibits a strong similarity to the Kalman filter prediction step. We use a similar procedure for the measurement corrections. Specifically, we approximate the measurement model using a first-order Taylor series expansion about the predicted point, ÌÎŒ t , to yield: h(x t )â h( ÌÎŒ t ) + H t (x t â ÌÎŒ t ), where H t = â x h( ÌÎŒ t ) is the Jacobian of h(x t ) evaluated at ÌÎŒ t . We then write the probabilistic measurement model using this approximation as: p(z t | x t ) = 1 p det(2ÏR t ) exp â 1 2 âz †t R â1 t âz t , whereâz t = z t â h( ÌÎŒ t )â H t (x t â ÌÎŒ t ). The measurement update step of the EKF uses the nonlinear measurement model and the linear approximation to compute: ÎŒ t = ÌÎŒ t + K t (z t â h( ÌÎŒ t )), ÎŁ t = (Iâ K t H t ) Ì ÎŁ t , where the Kalman gain is K t = Ì ÎŁ t H †t (H t Ì ÎŁ t H †t + R t ) â1 . Again, we can see that this is very similar to the Kalman filter measurement update step. We combine the EKF prediction and measurement update steps together in the overall EKF algorithm definition in Algorithm 12.2. Compare this to Algo- rithm 12.1 and you will notice only the small difference that the EKF uses a combination of the nonlinear models and linear approximations from their Jaco- bians. approximate filters for state estimation 309 Algorithm 12.2: Extended Kalman Filter (EKF) Data: ÎŒ tâ1 ,ÎŁ tâ1 , u t , z t Result: ÎŒ t ,ÎŁ t // Linearize dynamics at (ÎŒ tâ1 , u t ) F t â â f /âx (ÎŒ tâ1 , u t ) L t â â f /âΔ (ÎŒ tâ1 , u t ) // Prediction ÌÎŒ t â f (ÎŒ tâ1 , u t ) Ì ÎŁ t â F t ÎŁ tâ1 F †t + L t Q t L †t // Linearize measurement at ÌÎŒ t H t â âh/âx ÌÎŒ t // Innovation and gain Ìz t â z t â h( ÌÎŒ t ) S t â H t Ì ÎŁ t H †t + R t K t â Ì ÎŁ t H †t S â1 t // Correction ÎŒ t â ÌÎŒ t + K t Ìz t // Joseph-form covariance update (numerically robust) ÎŁ t â (Iâ K t H t ) Ì ÎŁ t (Iâ K t H t ) †+ K t R t K †t return ÎŒ t ,ÎŁ t Intuition. The EKF carries out a first-order approximation of the nonlinear mod- els around the mean estimate, which is the most plausible operating point. The Kalman filter formulas then apply to this locally linear surrogate. The quality of the update hinges on the local linearity of f and h around the chosen lineariza- tion points and on the fidelity of the noise covariances. If the robot frequently operates in regions where the models are strongly nonlinear over the uncertainty region, or where uncertainty is large, the linearization may be poor and the EKF can become inconsistent or even diverge. Practical considerations. The EKF provides more accurate results than the Kalman filter in many applications due to its ability to consider more general nonlinear models. However, because it relies on a first-order Taylor expansion, it can per- form poorly or diverge if the models are highly nonlinear over the region of uncertainty or if the linearization point is far from the true state. The EKF also inherits the unimodal Gaussian belief representation from the Kalman filter and thus cannot represent multi-hypothesis uncertainty. In practice, successful EKF deployments rely on good initializations, careful tuning of the noise covariances, and models that are only mildly nonlinear over the region of interest. 310 principles of robot autonomy 12.3.2 Unscented Kalman Filter (UKF) The UKF improves upon a core weakness of the EKF that first-order lineariza- tion can poorly capture how nonlinear transformations distort a Gaussian belief. Instead of linearizing the nonlinear functions f and h, the UKF approximates the belief distribution itself. It does so by propagating a carefully chosen set of deterministically sampled points called sigma points through the true nonlinear functions, and then recomputing the mean and covariance from the transformed points. Intuitively, if we start from a Gaussian belief, we can summarize it with a small cloud of representative points that capture its shape. If we pass those points through the nonlinear dynamics and sensing models, we can then reconstruct a new mean and covariance that more accurately reflect how the nonlinearity distorts the belief. This idea is formalized by the unscented transform. Unscented transform (UT). Consider an n-dimensional Gaussian N (ÎŒ,ÎŁ) and a nonlinear function g(·). The UT constructs a deterministic set of 2n + 1 sigma pointsX (i) that capture the mean and covariance of the Gaussian: X (0) = ÎŒ, X (i) = ÎŒ + c i , X (i+n) = ÎŒâ c i , i = 1, . . . , n, where the vectors c i are the columns of a matrix square root of (n + λ)ÎŁ (for example, a Cholesky factor). The scalar λ controls the spread of the sigma points via tuning parameters (α, ÎČ, Îș), with: λ = α 2 (n + Îș)â n. Each sigma point is assigned an associated weight for the mean and covari- ance,W (i) m , W (i) c . A common choice is: W (0) m = λ n + λ ,W (0) c = λ n + λ + (1â α 2 + ÎČ), and for i > 0: W (i) m = W (i) c = 1 2(n + λ) . Here, α â (0, 1] controls how far the sigma points spread from the mean, ÎČ in- corporates prior knowledge about the distribution, and Îș is an additional scaling parameter. To apply the UT to a nonlinear transformation g(·), we simply pass the sigma points through g: Y (i) = g(X (i) ), and then reconstruct the mean and covariance of the transformed variable as: Ìy = â i W (i) m Y (i) ,P y = â i W (i) c Y (i) â Ìy Y (i) â Ìy †. The cross-covariance between x and y = g(x) is computed analogously. This cross-covariance quantifies how deviations in the state around ÎŒ correlate with deviations in the transformed quantity around Ìy, and it is exactly the object needed to form the Kalman gain in the UKF. approximate filters for state estimation 311 Accuracy of the UT. For Gaussian priors, the UT matches the mean and covari- ance of y = g(x) to at least second order 6 in a Taylor expansion of g, without 6 For certain choices of parameters, can be accurate to third order. evaluating any Jacobians. In contrast, the EKFâs linearization is only first-order accurate. Intuitively, the symmetry of the sigma points around ÎŒ causes many first- and second-order terms in the Taylor series to cancel in the weighted sums, leaving high-order terms as the dominant approximation error. We refer the reader to Julier and Uhlmann [60] for a detailed analysis. From a practical point of view, the UT gives us a plug-and-play tool. We can feed in a Gaussian belief and a nonlinear function, and it returns an updated Gaussian that better reflects the nonlinear mapping than a first-order Taylor se- ries. UKF recursion (additive noise). Algorithm 12.3 can be read as a Kalman filter where the prediction and measurement steps are implemented via the unscented transform. For the common case with additive process and measurement noise, the UKF proceeds as follows: 1. Sigma point generation: from the current Gaussian belief (ÎŒ tâ1 ,ÎŁ tâ1 ), construct a set of sigma points and weights using the UT. 2. Dynamics propagation: pass each sigma point through the nonlinear state tran- sition model f (·, u t ) to obtain predicted points. Compute the predicted mean ÌÎŒ t and covariance Ì ÎŁ t by weighted averaging, and add the process noise covari- ance Q t . 3. Measurement prediction: apply the measurement model h(·) to each predicted sigma point. Compute the predicted measurement mean Ëz t , the measurement covariance S t , and the cross-covariance between state and measurement, P f h . 4. Update: form the Kalman gain K t = P f h S â1 t , update the mean via ÎŒ t = ÌÎŒ t + K t (z t â Ëz t ), and update the covarianceÎŁ t = Ì ÎŁ t â K t S t K †t . For non-additive noise models, we typically augment the state vector with noise variables and construct sigma points in this augmented space. EKF vs. UKF: when to use each. The choice between EKF and UKF depends on the specific problem characteristics, including the degree of nonlinearity, the avail- ability and reliability of Jacobians, and computational constraints. âą The EKF algorithm is appropriate when the dynamics and measurement mod- els f and h are mildly nonlinear, their Jacobians are straightforward to compute and accurate, and the computational budget is tight. The EKF is first-order ac- curate in the sense that it relies on a linear approximation of f and h around the mean and can be biased when nonlinearities are strong over the uncertainty region. âą The UKF algorithm excels when nonlinearities in the dynamics or sensing are substantial, derivatives are difficult to obtain or unreliable, or measurements 312 principles of robot autonomy Algorithm 12.3: Unscented Kalman Filter (UKF) Data: ÎŒ tâ1 ,ÎŁ tâ1 , u t , z t , (α, ÎČ, Îș) Result: ÎŒ t ,ÎŁ t // Build sigma point set from (ÎŒ tâ1 ,ÎŁ tâ1 ) λâ α 2 (n + Îș)â n X (0) â ÎŒ tâ1 W (0) m â λ n+λ W (0) c â λ n+λ + (1â α 2 + ÎČ) W (>0) m â W (>0) c â 1 2(n+λ) C â Cholesky((n + λ)ÎŁ tâ1 ) for i = 1 to n do X (i) â ÎŒ tâ1 + c i X (i+n) â ÎŒ tâ1 â c i // Propagate sigma points through dynamics for i = 0 to 2n do Y (i) â f (X (i) , u t ) // Predicted mean and covariance ÌÎŒ t â â i W (i) m Y (i) P f f â â i W (i) c Y (i) â ÌÎŒ t Y (i) â ÌÎŒ t â€ Ì ÎŁ t â P f f + Q t // Propagate sigma points through measurement model for i = 0 to 2n do Z (i) â h(Y (i) ) // Predicted measurement mean and covariances Ìzâ â i W (i) m Z (i) P h â â i W (i) c Z (i) â Ìz Z (i) â Ìz †P f h â â i W (i) c Y (i) â ÌÎŒ t Z (i) â Ìz †// Compute gain and correction S t â P h + R t K t â P f h S â1 t ÎŒ t â ÌÎŒ t + K t (z t â Ìz) // Covariance update ÎŁ t â Ì ÎŁ t â K t S t K †t return ÎŒ t ,ÎŁ t are strongly nonlinear (for example, bearing-only measurements). For Gaus- sian priors, the UKF is second-order accurate in the mean and covariance, and often higher, and typically captures the beliefâs evolution more accurately than the EKF in these regimes, at the cost of evaluating the dynamics and measure- ment models at 2n + 1 sigma points per step. Example 12.3.1 (Rangeâbearing update (nonlinear sensing)). To see the EKF and approximate filters for state estimation 313 UKF in action, consider a planar robot whose state is its pose x t = [ p x , p y , Ξ] †and that observes a fixed landmark at position â = [â x , â y ] †. The robot carries a sensor that measures the range and bearing to the landmark: h(x t ) = ïŁź ïŁ° q (â x â p x ) 2 + (â y â p y ) 2 atan2(â y â p y , â x â p x )â Ξ ïŁč ïŁ» . Both the square root and the atan2 function make this a strongly nonlinear mea- surement model, especially when the robot is close to the landmark or uncertain in orientation. EKF approach. In the EKF, we handle this nonlinearity by linearizing h around the predicted mean ÌÎŒ t = [ Ì p x , Ì p y , Ì Îž] †. The resulting Jacobian: H t = âh âx ÌÎŒ t , captures how small changes in pose affect the range and bearing. Intuitively, moving the robot towards the landmark shortens the range, while rotating the robot changes the bearing. The measurement innovation is: Ìz t = z t â h( ÌÎŒ t ), with the bearing residual wrapped into (âÏ, Ï] to avoid jumps across the an- gle discontinuity. The standard EKF correction step then uses H t , R t , and the predicted covariance to compute the Kalman gain and update the state. Algorithm 11 shows a Python implementation of this EKF update. The struc- ture closely mirrors the math: predict, linearize, compute the innovation and its covariance, compute the gain, and correct. UKF approach. The UKF handles the same problem by avoiding Jacobians alto- gether. Starting from the current Gaussian belief, it constructs sigma points in the pose space, propagates them through the nonlinear motion model and the nonlinear rangeâbearing sensor model, and then recomputes the predicted mean, covariance, and cross-covariance from the transformed sigma points. Because the UKF sees the full curvature of the measurement function through these propagated points, it can more accurately capture how the rangeâbearing observation tightens or shifts the belief, especially in regimes where the EKFâs linear approximation is poor. Algorithm 12 provides a Python implementation of the UKF update for this example. Its structure is analogous to the EKF code, but with explicit sigma- point generation, propagation, and weighted recombination in place of Jacobian calculations. Comparing the EKF and UKF. The two filters have the same high-level structure: prediction, measurement prediction, computation of an innovation and its co- variance, formation of a Kalman gain, and correction of the mean and covariance. The key differences are: 314 principles of robot autonomy Range-Bearing Robot EKF Update def ekf _ update(prior _ mean, prior _ cov, z, R, landmark): # Prediction F = robot _ jacobian(prior _ mean) pred _ mean = robot _ dynamics(prior _ mean) pred _ cov = F @ prior _ cov @ F.T # Innovation and gain H = range _ bearing _ jacobian(prior _ mean, landmark) z _ hat = range _ bearing(pred _ mean, landmark) innov = np.array([z[0] - z _ hat[0], wrap _ angle(z[1] - z _ hat[1])]) S = H @ prior _ cov @ H.T + R K = pred _ cov @ H.T @ np.linalg.inv(S) # Correction mean = pred _ mean + K @ innov I = np.eye(3) cov = (I - K @ H) @ pred _ cov @ (I - K @ H).T + K @ R @ K.T mean[2] = wrap _ angle(mean[2]) return mean, cov Algorithm 11: EKF update for the robot with a range-bearing sensor described in Example 12.3.1. The code for this example is available in the repository github.c om/StanfordASL/pora-exercises in the notebook ch12/ekf _ ukf _ range _ bearing .ipynb. approximate filters for state estimation 315 Range-Bearing Robot UKF Update def ukf _ update(prior _ mean, prior _ cov, z, R, landmark): # Compute sigma points and predicted mean/covariance X, Wm, Wc = compute _ sigma _ points(prior _ mean, prior _ cov) Y = np.array([robot _ dynamics(x) for x in X]) pred _ mean = np.zeros(3) pred _ mean[0] = np.sum(Wm * Y[:,0]) pred _ mean[1] = np.sum(Wm * Y[:,1]) pred _ mean[2] = angle _ mean(Wm, Y[:,2]) # Compute predicted measurement Z = np.array([range _ bearing(y, landmark) for y in Y]) pred _ z = np.zeros(2) pred _ z[0] = np.sum(Wm * Z[:,0]) pred _ z[1] = angle _ mean(Wm, Z[:,1]) # Compute predicted covariances Pff = np.zeros((3,3)) Phh = np.zeros((2,2)) Pfh = np.zeros((3,2)) for i in range(Y.shape[0]): dz = np.array([Z[i,0] - pred _ z[0], wrap _ angle(Z[i,1] - pred _ z[1])]) dy = Y[i] - pred _ mean dy[2] = wrap _ angle(dy[2]) Pff += Wc[i] * np.outer(dy, dy) Phh += Wc[i] * np.outer(dz, dz) Pfh += Wc[i] * np.outer(dy, dz) # Compute gain and correction pred _ cov = Pff S = Phh + R K = Pfh @ np.linalg.inv(S) innov = np.array([z[0] - pred _ z[0], wrap _ angle(z[1] - pred _ z[1])]) mean = pred _ mean + K @ innov mean[2] = wrap _ angle(mean[2]) cov = pred _ cov - K @ S @ K.T return mean, cov Algorithm 12: UKF update for the robot with a range-bearing sensor described in Example 12.3.1. The code for this example is available in the repository gith ub.com/StanfordASL/pora-exercises in the notebook ch12/ekf _ ukf _ range _ bea ring.ipynb. 316 principles of robot autonomy 1. The EKF uses Jacobians (F t , H t ) and linear formulas for the prediction and update, whereas the UKF uses explicit sigma-point generation, propagation, and weighted recombination. 2. The UKF recomputes predicted covariances and cross-covariances by weighted outer products of transformed sigma points instead of using linear covariance propagation. Discussion. The EKF and UKF are two sides of the same Gaussian belief coin. The EKF linearizes the models and keeps the belief exact as a Gaussian, while the UKF keeps the models exact and approximates the belief via sigma points. Both are grounded in the Bayes filter and both maintain a single Gaussian over the state. This example also highlights when the extra effort of the UKF is worthwhile. If the robotâs pose uncertainty is small and the landmark is far away, the mea- surement model is nearly linear locally and the EKF performs well at lower com- putational cost. If the robot is close to the landmark, has substantial heading uncertainty, or the geometry is strongly nonlinear in other ways, the UKF more faithfully captures how the observation reshapes the belief. In the repository github.com/StanfordASL/pora-exercises, the notebook ch12/ekf _ ukf _ range _ bearing.ipynb visualizes the EKF and UKF updates for this problem and lets you experiment with different levels of nonlinearity and uncertainty. 12.4 Non-parametric Filters: From Grids to Particles Parametric filters gain efficiency by committing to a fixed belief shape 7 . This 7 As discussed earlier in this chapter, usually a Gaussian distribution. commitment is powerful when the world behaves roughly as assumed, but it can be too rigid when uncertainty is multimodal, the dynamics or sensing are strongly nonlinear, or data association is ambiguous. Non-parametric filters remove this structural assumption and approximate the belief directly. They typically do so either by: 1. discretizing the state space into bins (histogram filters), or 2. representing the belief with samples (particle filters). Both methods are direct approximations of the Bayes filter and trade additional computation for representational flexibility. Up to this point, we have mostly assumed that beliefs can be summarized by a single Gaussian. In many robotics problems this is not realistic 8 . A unimodal 8 For example, in global localization a mobile robot may initially have no idea which part of a building it is in, leading to several widely separated hypotheses. Gaussian cannot represent such a situation well because it spreads probability mass between the plausible hypotheses rather than concentrating it at those lo- cations. Non-parametric filters are designed to address precisely this kind of scenario. approximate filters for state estimation 317 In the remainder of this section, we introduce two widely used non-parametric filters that build on the Bayes filter from Chapter 11. We first discuss the histogram filter in Section 12.4.1, which discretizes the state space, and then the particle filter in Section 12.4.2, which represents the belief with samples. 12.4.1 Histogram Filter The histogram filter is a non-parametric filter that can be viewed as an extension of the discrete Bayes filter from Chapter 11 to continuous state spaces. It proceeds by discretizing the continuous state space into a finite number of regions and then representing the belief distribution as a set of probabilities over these regions. Conceptually, we overlay a grid on the state space and store one number per grid cell representing the probability that the state lies in that cell. Mathematically, for the random state vector X, we discretize the continuous state space,X , into a finite set of regions, or bins, such that: X = b 1 âȘ b 2 âȘ . . .âȘ b K , where b k is the k-th bin and K is the total number of bins. For example, if a one-dimensional state variable takes values in the interval [0, 1], the interval can be divided into a set of equally spaced sub-intervals. The belief distribution bel(x t ) is then represented by assigning a probability mass p k,t to each bin b k . This quantity represents the probability that the state x t lies inside bin b k at time t. The histogram representation can also be interpreted as a piecewise-constant probability density function, where the density is constant within each bin and given by: p(x t ) = p k,t |b k | , x t â b k , where|b k | denotes the area or volume of the bin. 9 9 By construction â k p k,t = 1. In practice, the probabilities are renormalized after the correction step to maintain this prop- erty. To connect the histogram filter to the underlying continuous models, it is useful to define the binwise transition and likelihood that the filter approxi- mates. Let the continuous transition and measurement models be p(x t | x tâ1 , u t ) and p(z t | x t ), respectively. We define the corresponding binwise quantities as: T (t) ki : = p(x t â b k | x tâ1 â b i , u t ) = 1 |b i | Z x tâ1 âb i Z x t âb k p(x t | x tâ1 , u t ) dx t dx tâ1 , (12.6) L (t) k : = p(z t | x t â b k ) = 1 |b k | Z x t âb k p(z t | x t ) dx t .(12.7) In principle this formulation is exact, however, in practice, directly evaluating the integrals is usually intractable. A common approximation associates each bin b k with a representative state defined as the bin mean: Ëx k : = 1 |b k | Z b k x dx.(12.8) 318 principles of robot autonomy Algorithm 12.4: Histogram Filter Data: p k,tâ1 , u t , z t Result: p k,t foreach k do p k,t â â i p(b k,t | b i,tâ1 , u t ) p i,tâ1 p k,t â p(z t | b k,t ) p k,t // Normalization (to enforce â k p k,t = 1; use log-weights if probabilities are very small) η â ( â k p k,t ) â1 foreach k do p k,t â η p k,t return p k,t Using these mean states, we approximate the binwise transition model p(b k,t | b i,tâ1 , u t ) by: p(b k,t | b i,tâ1 , u t )â η|b k | p( Ëx k,t | Ëx i,tâ1 , u t ),(12.9) where p( Ëx k,t | Ëx i,tâ1 , u t ) is the original (continuous) state transition model eval- uated at the mean bin states, and η is a normalization constant. 10 We discretize 10 If the bin areas |b k | are equal, we can absorb this term into the normalization constant η. For higher fidelity, we may sample multiple points per bin and av- erage. the probabilistic measurement model in a similar way: p(z t | b k,t )â p(z t | Ëx k,t ),(12.10) so that the measurement probability associated with bin b k is approximated by the measurement probability at the representative state Ëx k . 11 11 For angular variables such as head- ings, ensure the representative state re- spects periodicity and wrap residuals appropriately. Once we have discretized the state space with bins b k and approximated the transition and measurement models using the bin mean states, the histogram filter follows the same structure as the discrete Bayes filter in Algorithm 11.2. We summarize it in Algorithm 12.4. A few implementation insights make the histogram filter more practical: âą The transition matrix is typically sparse. Most motion models move probability only to nearby bins. Rather than looping over all bin pairs (i, k), it is often more efficient to loop over each b i and distribute its mass to a small set of neighboring b k . âą If p(x t | x tâ1 , u t ) is shift-invariant 12 , the prediction step becomes a discrete 12 For example, additive Gaussian mo- tion in a grid. convolution. In such cases the computation can be accelerated using separable kernels or fast Fourier transforms in one- or two-dimensional grids. âą At domain boundaries, boundary conditions must be chosen to match the physical system. Common choices include reflecting, absorbing, or wrap- around boundaries. 13 13 For example, orientation variables on S 1 are naturally periodic and therefore use wrap-around boundaries. Like the discrete Bayes filter, the main disadvantage of the histogram filter is that it can become computationally intractable when the number of bins grows approximate filters for state estimation 319 large. This occurs when high spatial resolution is required for accuracy or when the state space is high dimensional. For example, in a robot localization problem where we estimate a planar pose (x, y, Ξ), discretizing x and y at 1 meter resolu- tion in a modestly sized building and Ξ at 1 ⊠resolution can easily yield hundreds of thousands of bins. 14 14 Cost scales roughly as O(K n nbr ) per step, where n nbr is the number of motion-neighbor bins (often small). Memory is O(K). This âcurse of dimen- sionalityâ is a primary motivation for particle filters. Example 12.4.1 (One-dimensional motion with Gaussian noise + range sensing). Consider a robot moving along a line segment [0, L]. We discretize this interval into K equal bins of widthâ = L/K, with bin k spanning [x â k , x + k ] and centroid Ë x k = (kâ 1 2 )â. The robot follows a simple additive motion model and receives a noisy range measurement to the wall at the origin: x t = x tâ1 + u t + Δ t ,Δ t âŒN (0, Ï 2 u ),z t = x t + Îœ t ,Îœ t âŒN (0, Ï 2 z ). Prediction. After applying control u t , the robotâs position distribution is shifted and blurred according to motion noise. For each pair of bins (i, k), the probability of moving from bin i to bin k can be approximated by the Gaussian mass over b k centered at Ë x i + u t : T (t) ki âΊ x + k â( Ë x i +u t ) Ï u âΊ x â k â( Ë x i +u t ) Ï u , whereΊ is the standard normal CDF. The predicted beliefp k,t is obtained by summing these contributions over all bins i. Correction. Given a measurement z t , each bin is reweighted according to how likely its centroid Ë x k is under the measurement model: L (t) k âN z t ; Ë x k , Ï 2 z . The posterior is then p k,t â L (t) k p k,t , followed by normalization so that â k p k,t = 1. Discussion. This process naturally captures both unimodal and multimodal be- liefs. Starting from a uniform prior, the filter may, for some time, maintain several peaks if the measurement is ambiguous 15 . As more controls and measurements 15 For example, if the environment has repeated structures. accumulate, inconsistent modes gradually lose probability, and the posterior col- lapses to a single sharp peak near the true location. In this way, the histogram fil- ter provides a simple yet powerful tool for global localization in low-dimensional settings. 12.4.2 Particle Filter The particle filter is a non-parametric filter that is often more computationally tractable than the histogram filter for continuous, higher-dimensional state spaces. Rather than discretizing the state space a priori, it represents the belief distribu- tion by a finite set of samples from the state space, called particles. 16 The key idea 16 The particle filter is sometimes re- ferred to as a Monte Carlo algorithm due to its sampling-based nature. is to place more particles in regions of high probability and fewer in regions of low probability so that the particle set adapts to the shape of the belief. 320 principles of robot autonomy We define the set of particles at time t as: P t : =x [1] t , x [2] t , . . . , x [K] t ,(12.11) where x [k] t is the k-th particle. Each particle x [k] t represents a hypothesis about the true state x t , and regions of the state space with more particles correspond to regions of higher probability. Ideally, the particles are distributed according to the current belief: x [k] t ⌠bel(x t ), but in practice this holds only approximately for finite K. 17 17 As K ââ, the empirical distribution of the particles converges to the true be- lief under mild conditions. In many ap- plications, on the order of K â 10 3 parti- cles already provides useful approxima- tions. Following the Bayes filter paradigm, the particle filter updates the prior belief, represented byP tâ1 , via a prediction step and a measurement update step. Prediction (sampling through the dynamics). For each particle x [k] tâ1 in the prior set, we draw a new sample from the state transition model: Ìx [k] t ⌠p(x t | x [k] tâ1 , u t ). This step plays the role of the motion update where the cloud of particles is pushed forward according to the control and process noise. The resulting set Ìx [k] t approximates the predicted belief bel(x t ). Measurement weighting and resampling. We next assess how well each predicted particle is supported by the new measurement z t . For each predicted particle, we compute a weight: w [k] t = p(z t | Ìx [k] t ), so that particles that are more consistent with the measurement receive larger weights. We collect the predicted particles and their weights into a weighted set Ì P t =( Ìx [k] t , w [k] t ), which approximates the unnormalized posterior. The particle filterâs measurement update step then consists of resampling (with replacement) a new set of K particles from Ì P t , according to the normalized weights w [k] t . Particles with large weights are likely to be selected many times, while particles with negligible weights may disappear. The resampled set P t approximates the updated belief bel(x t ). We summarize the algorithm in Algorithm 12.5 and illustrate a few iterations of the particle filter for a simple robot localization problem in Section 12.4.2. The resampling step is important for more than just incorporating the measure- ment. Without resampling, repeated multiplication of weights would gradually concentrate probability on a tiny subset of particles, while most particles would have negligible weight and effectively be wasted. This phenomenon is known as particle degeneracy. Resampling combats degeneracy by repeatedly discarding low-weight particles and replicating high-weight particles, keeping the effective sample size roughly constant. approximate filters for state estimation 321 Algorithm 12.5: Particle Filter Data: P tâ1 , u t , z t Result: P t Ì P t ââ for k = 1 to K do Sample Ìx [k] t ⌠p(x t | x [k] tâ1 , u t ) w [k] t â p(z t | Ìx [k] t ) Ì P t â Ì P t âȘ Ìx [k] t , w [k] t // Resampling step (with replacement) according to weights P t ââ for k = 1 to K do Draw index i with probabilityâ w [i] t P t âP t âȘ Ìx [i] t returnP t This effect has a nice interpretation in evolutionary terms: particles that ex- plain the data well âsurvive and reproduceâ, while those that do not are gradu- ally removed. From a computational perspective, resampling focuses particles in high-probability regions of the state space and reduces the number of particles required for a given level of accuracy. 12.5 Summary In this chapter, we examined practical approximations to the Bayes filter for con- tinuous and high-dimensional state estimation problems in robotics. We began by motivating the need for tractable filtering methods and introduced two broad families of approximate filters, parametric and non-parametric, each with its own strengths and weaknesses. Parametric filters, such as the Kalman filter and its nonlinear extensions, the EKF and the UKF, represent the belief as a Gaussian distribution, enabling effi- cient recursive updates of mean and covariance. We discussed how these filters exploit linear or locally linear models to achieve computational efficiency, along with their limitations in representing multimodal or highly nonlinear beliefs. We then introduced non-parametric filters, including the histogram and par- ticle filters, which relax parametric assumptions and directly approximate the belief distribution through discretization or sampling. These methods trade ad- ditional computation for representational flexibility, making them effective in sit- uations involving global uncertainty, nonlinear dynamics, or non-Gaussian noise. Section 12.5 summarizes the main characteristics of these approaches and high- lights their complementary strengths. In practice, robotic systems often combine elements of both families in order to balance accuracy, robustness, and computa- tional efficiency. 322 principles of robot autonomy Figure 12.1:A particle filter used for robot localization. We first update the initial set of par- ticles according to the transition model, and then weight them according to the observation. Fi- nally, we generate a new set of particles through weighted re- sampling. High-level decision guide. It is useful to relate common robotics scenarios to ap- propriate filter architectures. Kalman-style filters are well suited for local tracking problems with smooth motion and reliable sensors, whereas histogram or parti- cle filters are better suited for global localization, ambiguous data association, or highly nonlinear environments. 1. If the robotâs dynamics or sensing models are mildly nonlinear and beliefs stay near unimodalâ EKF. 2. If the Jacobians of the dynamics or sensor models are hard to compute or their nonlinearities are significantâ UKF (or square-root UKF). 3. If there is global or multimodal uncertainty, or severe non-Gaussian noise â Particle filter. 4. If the state is low-dimensional and a map or grid is availableâ Histogram filter. approximate filters for state estimation 323 Parametric (KF / EKF / UKF)Non-parametric (Histogram / Particle) BeliefSingle Gaussian (mean, covari- ance) Grid masses or weighted sam- ples NonlinearityEKF (linearize), UKF (sigma points) Native; no Jacobians needed MultimodalityPoor (unimodal)Natural (multi-peak) DimensionalityScales well with state dimHistogram:suffers;Particle: scalable with K ComputationCheap per stepHistogram:grows with bins; Particle: O(K) When it shinesSmooth models, near-Gaussian noise, good observability Ambiguity, strong nonlineari- ties, non-Gaussian noise, global localization PitfallsLinearization bias (EKF), covari- ance inconsistency Degeneracywithoutresam- pling; sample impoverishment Table 12.1: Parametric vs. Non- parametric Filters (at a glance) To learn more. For a comprehensive and accessible treatment of probabilistic state estimation and filtering methods, readers are encouraged to consult the classic text by Thrun, Burgard, and Fox [148]. Detailed derivations and discussions of Kalman filtering theory can also be found in Maybeck [93], while Julier and Uhlmann [60] provide the seminal introduction to the Unscented Kalman Fil- ter. For further reading on non-parametric filters and their applications to robot localization, see Dellaert et al. [30]. A modern perspective that unifies filtering, smoothing, and mapping under a probabilistic framework is presented in Carlone et al. [24], which connects the foundations of state estimation to contemporary SLAM and spatial-AI systems. 12.6 Exercises The starter code for the exercises provided below is available online through GitHub. To get started, download the code by running in a terminal window: git clone https://github.com/StanfordASL/pora-exercises.git We denote Problems requiring hand-written solutions and coding in Python with and, respectively. Problem 1: Kalman Filter for Landmark Localization In this exercise, you will implement a Kalman Filter to localize a set of fixed landmarks given knowledge about a robotâs motion. Specifically, consider an environment where there are four landmarks, and we define the state of landmark 324 principles of robot autonomy positions as the vector: x m t = h x m 1 t y m 1 t x m 2 t y m 2 t x m 3 t y m 3 t x m 4 t y m 4 t i †, which we assume we can directly measure (i.e. z t = x t ) with a noisy sensor. In the file ch12/exercises/kalman _ filter.ipynb, perform the following tasks: 1. Define the state transition matrix A from Equation (12.3) and observation ma- trix C from Equation (12.4) for the Kalman filter algorithm. The matrix A should correspond to the motion of the landmarks, which we assume are sta- tionary, and C should represent the direct measurement of the landmark posi- tions. 2. Implement the Kalman filter predict and update steps to compute the means and covariances at each time step. 3. Run the provided code to see the results. approximate filters for state estimation 325 References [24]L. Carlone et al., eds. SLAM Handbook. From Localization and Mapping to Spatial Intelligence. Cambridge University Press, 2026. [30]F. Dellaert et al. âMonte carlo localization for mobile robotsâ. In: Proceed- ings 1999 IEEE international conference on robotics and automation (Cat. No. 99CH36288C). Vol. 2. IEEE. 1999, p. 1322â1328. [60]S. J. Julier and J. K. Uhlmann. âNew extension of the Kalman filter to nonlinear systemsâ. In: Signal processing, sensor fusion, and target recognition VI. Vol. 3068. Spie. 1997, p. 182â193. [93]P. S. Maybeck. Stochastic models, estimation, and control. Vol. 3. Academic press, 1982. [148]S. Thrun, W. Burgard, and D. Fox. Probabilistic Robotics. MIT Press, 2005. 13 Robot Localization The filtering algorithms developed in Chapter 11 and Chapter 12 provide general tools for estimating the state of a dynamical system from noisy measurements. In mobile robotics, one of the most important instances of this general problem is localization: estimating a robotâs pose with respect to a map of the environment. In this chapter, we specialize the Bayesian filtering framework to the robot localization problem. We begin in Section 13.1 with a taxonomy that organizes different localization scenariosâpose tracking versus global localization, static versus dynamic environments, active versus passive sensing, and single-robot versus multi-robot settings. This taxonomy provides a mental model for the kinds of problems that arise in practice. In Section 13.2, we then express localization formally as a Bayesian filtering problem in which the state is the robot pose and the map is an additional, known variable that influences both motion and measurements. This leads to map-aware state transition and measurement models, which we discuss in Section 13.3. Building on this formulation, in Section 13.4 we introduce Markov localization, a general Bayes filter for estimating a robotâs pose that explicitly incorporates a map. Markov localization is conceptually clean but rarely implemented directly; instead, it serves as a reference from which more specialized algorithms are de- rived. We then present two specializations of Markov localization. In Section 13.5, we develop an EKF localization algorithm for feature-based maps that are well suited to pose tracking with moderate uncertainty. Finally, in Section 13.6 we introduce Monte Carlo Localization (MCL), a particle filterâbased method that can handle global localization and strongly multimodal beliefs. Along the way, we discuss data association and practical considerations that arise in map-based localization. 13.1 A Taxonomy of Robot Localization Problems Robot localization problems can differ substantially depending on how much is known about the initial pose, how the environment evolves, how actions are chosen, and how many robots are involved. Before turning to specific algorithms, it is helpful to organize these variations using a taxonomy. This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 328 principles of robot autonomy AxisRepresentative casesTypical algorithmic implications Initial posePosetracking(local), global localization, kid- napped robot Tracking often amenable to uni- modal Gaussian filters; global and kidnapped scenarios typically re- quire non-parametric, multimodal beliefs (e.g., particle filters). EnvironmentStatic,slowly varying, strongly dynamic Static maps simplify modeling; dy- namic elements motivate robust measurement models and possibly explicit dynamic object tracking. Action selectionPassive,active (information-seeking) Passive use treats controls as given; active localization couples the filter with planning methods that trade off task progress and information gain. Numberof robots Single robot, multi-robot (cooperative) Multi-robotsettingsintroduce shared maps and inter-robot ob- servations,leading to coupled estimation problems and opportu- nities for improved robustness. Table 13.1: A taxonomy of robot localization problems along four key axes. We will use the term pose to refer to the robotâs position and orientation in a global coordinate frame. For a planar mobile robot, the pose is typically x t = (x t , y t , Ξ t ) at time t, where (x, y) is the planar position and Ξ is the heading angle. Pose tracking and global pose localization. One of the first distinctions among robot localization problems concerns what we assume we know about the initial pose: âą Pose tracking (local localization). In pose tracking problems, the initial pose is known with reasonably small uncertainty. For example, a robot may be placed at a known charging station or docking location, with a prior belief concen- trated in a small neighborhood. The goal is to maintain an accurate estimate of pose over time as the robot moves and senses, despite process and measure- ment noise. âą Global localization. In global localization, the robot has very little prior informa- tion about its initial pose. For instance, it may know only that it is somewhere within a building, but not which floor, corridor, or room. The belief over poses must therefore represent multiple plausible hypotheses, often spread over a large region of the map. âą Kidnapped robot problem. A challenge related to global localization arises when a robot that has been successfully tracking its pose is suddenly transported to a different, unknown location without its sensors or localization algorithm robot localization 329 being aware of the event. This kidnapped robot problem requires the localization method to recover from a grossly incorrect belief and reinitialize globally. Pose tracking problems are often amenable to unimodal Gaussian approxima- tions, such as EKF-based methods, whereas global localization and kidnapped robot scenarios typically require non-parametric methods capable of represent- ing multiple hypotheses, such as histogram or particle filters. Static and dynamic environment localization. A second axis in the taxonomy of robot localization concerns how the environment changes over time: âą Static environments. In the simplest case, the environment map is fixed and does not change. Walls and landmarks remain in place, and any moving objects (such as people) are ignored or treated as noise. Many foundational localization algorithms are developed under this assumption. âą Dynamic environments. In more realistic settings, parts of the environment change over time. Examples include doors that open and close, furniture that moves, or other agents that occupy the same space. In these cases, the map may be time varying or may explicitly model dynamic objects. In this chapter, we primarily consider localization with respect to a static map, while allowing for noisy measurements that may occasionally be corrupted by dynamic elements. Handling fully dynamic maps is closely related to SLAM and tracking, and will be revisited in Chapter 14. Active and passive localization. Localization algorithms also differ in how they interact with the environment: âą Passive localization. In passive localization, the algorithm treats the sequence of controls u 1:t as given; it does not attempt to choose actions to improve lo- calization performance. Many navigation systems fall into this category when localization is treated as a background process. âą Active localization. In active localization, the robot chooses its actions with the explicit goal of reducing uncertainty about its pose. For example, the robot may move to viewpoints that disambiguate map symmetries or collect mea- surements that are expected to be highly informative. The algorithms developed in this chapter apply in both settings. When used in an active framework, they provide the state and uncertainty estimates needed for planning information-seeking actions. Single and multi-robot localization. Finally, localization problems can involve one or multiple robots: 330 principles of robot autonomy âą Single-robot localization. A single robot must estimate its pose using on-board sensors and possibly an external reference, such as GPS, relative to a map that we assume is given. âą Multi-robot localization. In multi-robot settings, several robots localize simulta- neously, often sharing information. Robots may observe each other, exchange measurements, or maintain a shared map. This introduces additional structure and opportunities for cooperation, but also coupling between the individual localization problems. Throughout this chapter, we focus on single-robot localization in a known, largely static map. The methods we develop form the basis for more advanced multi-robot and SLAM systems. 13.2 Robot Localization via Bayesian Filtering In previous chapters, we introduced several well-known variations of the Bayes filter, including the parametric EKF and the non-parametric particle filter in Chapter 12. These algorithms propagate a belief distribution over a state x t using a probabilistic Markov state transition model and a probabilistic measurement model. Recall that for a general state-space model, the belief at time t is defined as: bel(x t ) : = p(x t | z 1:t , u 1:t ),(13.1) where z 1:t are all measurements up to time t and u 1:t are the applied controls. In map-based localization, the state x t represents the robot pose, and we as- sume that a map m of the environment is given. The map encodes information about the environment that constrains both how the robot can move and what it can measure. Our goal is to compute the posterior over robot poses conditioned on the map: bel(x t ) : = p(x t | z 1:t , u 1:t , m). To incorporate the map into the filtering framework, we modify both the state transition model and the measurement model. Specifically, in Section 13.3, we in- troduce a map-aware state transition model p(x t | x tâ1 , u t , m) that captures the fact that some poses are inconsistent with the environment. Then, in Section 13.3.1, we introduce a map-aware measurement model p(z t | x t , m) that reflects how lo- cal sensor readings depend on nearby structures in the map. The combination of these two models leads directly to the localization algorithms we introduce in Section 13.5 and Section 13.6. Map representation. Before modifying the probabilistic models, it is useful to briefly review how we represent maps in this chapter. We assume that the envi- ronment can be described by a collection of spatial entitiesâeither discrete land- marks or volumetric cellsâwith associated properties. Formally, we denote the robot localization 331 Figure 13.1: Two examples of location-based maps. Both rep- resent the map as a set of vol- umetric objects, which in these examples are cells. Figure 13.2: Two examples of feature-based maps. One uses a collection of lines and the other uses a graph representation of the empty spaces. map as: m =m 1 , m 2 , . . . , m N , where each element m i describes a portion of the environment. Depending on the application, m i may represent the occupancy state of a grid cell (occupied/free/unknown), the position of a point landmark, m i = (m i,x , m i,y ) in a global frame, or more complex attributes such as semantic labels or reflectiv- ity. We will use the generic term âmap elementâ for m i , and refer informally to these as âobjectsâ or âcellsâ when helpful. Two important map families arise frequently in mobile robotics: âą Feature-based maps, where each m i is a compact feature, such as a point land- mark or line segment, with precisely defined geometry. Section 13.2 shows two examples of feature-based maps, one represented by lines and another by a graph of nodes and edges. 1 1 Graph-based maps are generally re- ferred to as topological maps. âą Cell-based (occupancy grid) maps, where the environment is discretized into a grid of cells, each with an associated occupancy probability. This is closely related to the grid-based motion planning representations discussed in Sec- tion 4.2. Section 13.2 shows examples where the environment is represented by a set of volumetric cells. Cell-based maps trade off resolution and computational cost: smaller cells pro- vide finer spatial detail but increase memory usage and computation time, while larger cells reduce cost but blur detail and can misrepresent narrow passages. We refer back to the grid-based planning discussion in Section 4.2 for a deeper treat- ment of these trade-offs. In this chapter, we treat the choice of map representation as given, and focus on how the map enters the filtering equations. 332 principles of robot autonomy 13.3 Map-Aware State Transition Model The map influences how the robot can move. For example, a mobile robot cannot pass through walls or leave the known workspace. A purely kinematic transition model p(x t | x tâ1 , u t ) does not capture these constraints: it might assign nonzero probability to states that lie inside obstacles or outside the mapped area. To incorporate the map, we consider the map-aware transition model p(x t | x tâ1 , u t , m). Directly modeling this distribution for arbitrary maps is difficult, so we adopt an approximation that reuses the original transition model and a separate, map-induced prior. Specifically, we define p(x t | m) as the probability of a pose given only the map. This term characterizes how consistent a pose is with the environment. For example, p(x t | m) = 0 for a pose inside a wall or outside the mapped region. Using Bayesâ rule, we can approximate the map-aware transition model as: p(x t | x tâ1 , u t , m)â η p(x t | x tâ1 , u t ) p(x t | m) p(x t ) ,(13.2) where η is a normalization constant that ensures the left-hand side integrates to one. Equation (13.2) can be derived step-by-step by starting from Bayesâ rule: p(x t | x tâ1 , u t , m) = p(m| x t , x tâ1 , u t ) p(x t | x tâ1 , u t ) p(m| x tâ1 , u t ) . Since p(m| x tâ1 , u t ) does not depend on x t , we can absorb it into a constant η âČ : p(x t | x tâ1 , u t , m) = η âČ p(m| x t , x tâ1 , u t ) p(x t | x tâ1 , u t ). Next, we assume that the influence of the past on the map is captured entirely through the current pose x t : p(m| x t , x tâ1 , u t )â p(m| x t ). Intuitively, once we know the current pose, the specific path taken to get there does not provide additional information about the static map. 2 With this assump- 2 This approximation becomes more ac- curate as the time step between tâ 1 and t shrinks, so that the robot moves only a short distance between successive poses. tion: p(x t | x tâ1 , u t , m)â η âČ p(m| x t ) p(x t | x tâ1 , u t ). Applying Bayesâ rule again to p(m| x t ) gives: p(m| x t ) = p(x t | m) p(m) p(x t ) . Substituting into the previous expression and absorbing constant factors that do not depend on x t into a new normalization constant η yields: p(x t | x tâ1 , u t , m)â η p(x t | x tâ1 , u t ) p(x t | m) p(x t ) , which is Equation (13.2). robot localization 333 In this approximation, p(x t | m) is the probability of a state given only the map and captures the geometric consistency of that state with respect to the envi- ronment 3 . The factor p(x t | x tâ1 , u t ) encodes the motion model, and p(x t | m) 3 For example, p(x t | m) = 0 for a state x t that lies inside a wall or outside the known map. acts as a map-based correction that suppresses physically implausible states. From a computational perspective, Equation (13.2) is attractive because it al- lows us to reuse existing implementations of the motion model and incorporate the map through a relatively inexpensive correction: âą We first generate a kinematic prediction of the new pose using p(x t | x tâ1 , u t ), which depends only on the robot model and control. âą We then reweight or prune these predictions using p(x t | m), which often reduces to simple operations such as checking whether the pose lies in free space (for occupancy grids) or near a corridor (for semantic maps). Thus, the map-aware transition model modifies the likelihood of proposed poses without requiring a complete redesign of the underlying motion model. 13.3.1 Map-Aware Measurement Model The map also strongly influences sensor measurements. For instance, the range returned by a lidar beam depends on where that beam first intersects an obstacle in the map, and the bearing to a visual landmark depends on the landmarkâs position in the global frame. To reflect this dependence, we introduce a map- aware measurement model: p(z t | x t , m). In many systems, a measurement vector z t â R p consists of individual com- ponents, such as individual range beams or landmark observations. A common simplifying assumption is that the individual components of the measurement vector z t â R p are conditionally independent given the state and map. Under this assumption, we can factor the measurement model as: p(z t | x t , m) = p â i=1 p(z i t | x t , m).(13.3) The conditional independence assumption is not exact. For example, nearby range beams may be correlated if they hit the same object. However, this as- sumption greatly simplifies inference and works well in many practical settings. It allows us to process multiple sensor readings either in a batch or sequentially, using the same underlying model p(z i t | x t , m) for each component. 13.4 Markov Localization The first map-based localization algorithm we introduce is Markov localization. Markov localization applies the Bayes filter from Chapter 11 to the map-aware models introduced above. The belief over robot pose at time t is: bel(x t ) : = p(x t | z 1:t , u 1:t , m), 334 principles of robot autonomy and the map m is treated as known and fixed. Using the map-aware transition and measurement models p(x t | x tâ1 , u t , m) and p(z t | x t , m), the Bayes filter recursion becomes: Prediction:bel(x t ) = Z p(x t | x tâ1 , u t , m) bel(x tâ1 ) dx tâ1 , Correction:bel(x t ) = η p(z t | x t , m)bel(x t ), wherebel(x t ) denotes the predicted belief and η is a normalization constant cho- sen so that R bel(x t ) dx t = 1. We summarize this recursion in Algorithm 13.1. Algorithm 13.1: Markov Localization Data: bel(x tâ1 ), u t , z t , m Result: bel(x t ) foreach x t do bel(x t ) = R p(x t | x tâ1 , u t , m)bel(x tâ1 )dx tâ1 bel(x t ) = η p(z t | x t , m) bel(x t ) return bel(x t ) Conceptually, Markov localization has the same structure as the Bayes filter from Chapter 11: a prediction step that propagates the belief through the motion model, followed by a correction step that incorporates the latest measurement. The only difference is that both the prediction and correction now depend explic- itly on the map m. In its most general form, however, Markov localization is not directly compu- tationally tractable. The integral over all possible poses and the need to maintain an arbitrary belief function are prohibitive in high-dimensional or continuous state spaces. As in Chapter 12, the key to practicality is to choose a representation for the belief that is expressive enough for the problem at hand, yet structured enough to admit efficient computation. In Section 13.5, we adopt a unimodal Gaussian belief and derive an EKF-based localization algorithm, which is well suited for pose tracking with moderate un- certainty. In Section 13.6, we adopt a particle-based representation and derive Monte Carlo Localization, capable of handling global localization and multi- modal beliefs. 13.5 EKF Localization We now develop an EKF-based realization of Markov localization. The EKF lo- calization algorithm assumes that the belief over the robotâs pose can be approx- imated by a single Gaussian: bel(x t )âŒN (ÎŒ t ,ÎŁ t ), and applies the EKF prediction and update equations from Chapter 12 using map-aware models. This Gaussian structure significantly improves computa- robot localization 335 tional efficiency relative to the full Markov localization algorithm, at the cost of not being able to represent multiple well-separated pose hypotheses. 4 4 As in Chapter 12, this unimodality as- sumption is well suited to pose tracking but generally insufficient for global lo- calization. We assume the same nonlinear state transition model as in Section 12.3.1: x t = f (x tâ1 , u t ) + Δ t , where Δ t ⌠N (0, Q t ) is zero-mean Gaussian process noise. The corresponding state transition Jacobian is: F t =â x f (ÎŒ tâ1 , u t ),(13.4) where ÎŒ tâ1 is the mean of the previous belief bel(x tâ1 ). The main difference between the general EKF and EKF localization is the pres- ence of a feature-based map and a measurement model that relates robot poses to observed landmarks. We assume we have a map m of N point landmarks: m =m 1 , m 2 , . . . , m N ,m j = (m j,x , m j,y ), where each landmark m j is given by its two-dimensional location (m j,x , m j,y ) in the global coordinate frame. At time t the robot obtains a set of landmark mea- surements: z t =z 1 t , z 2 t , . . ., where each z i t is associated (implicitly or explicitly) with one landmark. Given a pose x t and the index j of the corresponding landmark, the measure- ment model is: z i t = h(x t , j, m) + ÎŽ t , where ÎŽ t ⌠N (0, R t ) models zero-mean Gaussian sensor noise, and h(·) encodes the expected range, bearing, or other features to landmark m j . For each measure- ment, the Jacobian of the measurement model with respect to the state is: H c i t t =â x h( ÌÎŒ t , c i t , m),(13.5) where ÌÎŒ t is the predicted mean from the EKF prediction step, and c i t is the index of the map feature associated with measurement i. A new challenge in feature-based localization is data association. Given a set of measurements z t , which landmark does each measurement correspond to? We denote the correspondence for measurement i at time t by c i t â 1, . . . , N + 1, where c i t = j means that measurement i corresponds to landmark m j , and c i t = N + 1 indicates that measurement i does not correspond to any known landmark 5 . 5 For example, due to a spurious detec- tion. We first consider the simpler case where we assume the correspondences c i t are known. 13.5.1 EKF Localization with Known Correspondences Assume for the moment that the correspondences c t = c 1 t , c 2 t , . . . are known. The EKF localization algorithm then resembles the standard EKF from Chap- ter 12, with two key modifications: 336 principles of robot autonomy Algorithm 13.2: EKF Localization with Known Correspondences Data: ÎŒ tâ1 ,ÎŁ tâ1 , u t , z t , c t , m Result: ÎŒ t ,ÎŁ t // Prediction (motion update) ÌÎŒ t = f (ÎŒ tâ1 , u t ) Ì ÎŁ t = F t ÎŁ tâ1 F †t + Q t // Correction: loop over landmark measurements foreach z i t do j = c i t S i t = H j t Ì ÎŁ t [H j t ] †+ R t K i t = Ì ÎŁ t [H j t ] †[S i t ] â1 ÌÎŒ t = ÌÎŒ t + K i t (z i t â h( ÌÎŒ t , j, m)) Ì ÎŁ t = (Iâ K i t H j t ) Ì ÎŁ t // Final posterior ÎŒ t = ÌÎŒ t ÎŁ t = Ì ÎŁ t return ÎŒ t ,ÎŁ t 1. The measurement model relates the pose to map features via h(x t , j, m). 2. Multiple landmark measurements are processed at each time step. Given the pose x t and the map m, and given known correspondences c t , the joint likelihood of the measurement set z t can be written as: p(z t | x t , c t , m). Using the conditional independence assumption from Equation (13.3)ânow ap- plied at the level of individual landmark measurementsâwe have: p(z t | x t , c t , m) = â i p(z i t | x t , c i t , m). Each factor p(z i t | x t , c i t , m) is a Gaussian distribution whose mean is given by h(x t , c i t , m) and whose covariance is R t . Because the prior over x t is Gaussian and each measurement likelihood term is Gaussian, the posterior remains Gaussian. Moreover, under the conditional independence assumption, we can apply the standard EKF measurement update sequentially for each z i t and obtain the same result as if we had processed all measurements in a single stacked update. This is a consequence of the fact that, for Gaussian models, multiplication of likelihood terms can be carried out in any order. Algorithm 13.2 summarizes the EKF localization recursion in the known-correspondence case. This algorithm provides a useful baseline: if we knew which measurement came from which landmark, EKF localization would be a straightforward special- ization of the standard EKF. The main additional difficulty in practice is that the robot localization 337 correspondences c i t are typically unknown and must be estimated jointly with the robot state. 13.5.2 EKF Localization with Unknown Correspondences In realistic scenarios, the correspondences c i t are not given and must be inferred from the data. One common approach is to estimate the correspondences using a maximum likelihood (MLE) criterion. At each time step, we choose c t = c i t to maximize the likelihood of the current measurements: Ëc t = arg max c t p(z t | c 1:t , m, z 1:tâ1 , u 1:t ). In words, we select the assignment of measurements to landmarks that makes the observed data most probable, given the map and the past history. To make this optimization tractable, we first marginalize over the unknown pose x t : p(z t | c 1:t , m, z 1:tâ1 , u 1:t ) = Z p(z t | x t , c 1:t , m, z 1:tâ1 , u 1:t ) p(x t | c 1:t , m, z 1:tâ1 , u 1:t )dx t , = Z p(z t | x t , c t , m) bel(x t )dx t , where we used the Markov assumption to simplify p(z t | x t , c 1:t , m, z 1:tâ1 , u 1:t ) = p(z t | x t , c t , m), and defined the predicted belief: bel(x t ) : = p(x t | c 1:t , m, z 1:tâ1 , u 1:t ). The term p(z t | x t , c t , m) is the measurement model with known correspon- dences. Using the conditional independence assumption from Equation (13.3), we can factor it as: p(z t | x t , c t , m) = â i p(z i t | x t , c i t , m). Substituting into the integral, we obtain: p(z t | c 1:t , m, z 1:tâ1 , u 1:t ) = Z bel(x t ) â i p(z i t | x t , c i t , m) dx t . Although the measurements are conditionally independent given x t , they be- come coupled after marginalizing over x t . A common approximation is therefore to select correspondences independently by scoring each measurement-landmark pairing under the predicted belief: Ë c i t = arg max c i t Z p(z i t | x t , c i t , m) bel(x t ) dx t . Under the Gaussian assumptions on the belief and measurement model, the integral above is a Gaussian distribution with mean and covariance: Z p(z i t | x t , c i t , m) bel(x t ) dx t âŒN Ëz c i t t , S c i t t , 338 principles of robot autonomy Algorithm 13.3: EKF Localization with Unknown Correspondences Data: ÎŒ tâ1 ,ÎŁ tâ1 , u t , z t , m Result: ÎŒ t ,ÎŁ t ÌÎŒ t = f (ÎŒ tâ1 , u t ) Ì ÎŁ t = F t ÎŁ tâ1 F †t + Q t foreach z i t do foreach landmark k in the map do Ëz k t = h( ÌÎŒ t , k, m) S k t = H k t Ì ÎŁ t [H k t ] †+ R t j = arg min k (z i t â Ëz k t ) †[S k t ] â1 (z i t â Ëz k t ) K i t = Ì ÎŁ t [H j t ] †[S j t ] â1 ÌÎŒ t = ÌÎŒ t + K i t (z i t â Ëz j t ) Ì ÎŁ t = (Iâ K i t H j t ) Ì ÎŁ t ÎŒ t = ÌÎŒ t ÎŁ t = Ì ÎŁ t return ÎŒ t ,ÎŁ t where: Ëz j t = h( ÌÎŒ t , j, m),S j t = H j t Ì ÎŁ t (H j t ) †+ R t . Maximizing the likelihood is therefore equivalent to choosing c i t to maximize the Gaussian density N (z i t | Ëz j t , S j t ), or equivalently to minimize the associated Mahalanobis distance: Ë c i t = arg min c i t d i,c i t t ,(13.6) where: d ij t = (z i t â Ëz j t ) †(S j t ) â1 (z i t â Ëz j t ),(13.7) is the Mahalanobis distance between the actual measurement z i t and its prediction Ëz j t under landmark j. In practice, additional validation gates are often imposed: if the minimum dis- tance d i Ë c i t t exceeds a threshold, the measurement is treated as unmatched (as- signed to the âno landmarkâ index N + 1) and excluded from the EKF update. Once the correspondences Ëc t have been determined, they are treated as known in Algorithm 13.3, yielding the full EKF localization algorithm with unknown correspondences. Example 13.5.1 (Differential drive robot with range and bearing measurements). Consider a differential drive robot with state x = [ x, y, Ξ ] †and a sensor that measures the range r and bearing Ï to landmarks m j â m in the robotâs local coordinate frame. We assume that at each time step the robot collects multiple measurements corresponding to different features: z t =[r 1 t , Ï 1 t ] †, [r 2 t , Ï 2 t ] †, . . ., where each measurement z i t contains the range r i t and bearing Ï i t . robot localization 339 Assuming the correspondences are known, the measurement model for the range and bearing of landmark j is: h(x t , j, m) = " q (m j,x â x) 2 + (m j,y â y) 2 atan2(m j,y â y, m j,x â x)â Ξ # .(13.8) The measurement Jacobian H j t corresponding to a measurement from landmark j is therefore: H j t = ïŁź ïŁ° â m j,x â Ì ÎŒ t,x â (m j,x â Ì ÎŒ t,x ) 2 +(m j,y â Ì ÎŒ t,y ) 2 â m j,y â Ì ÎŒ t,y â (m j,x â Ì ÎŒ t,x ) 2 +(m j,y â Ì ÎŒ t,y ) 2 0 m j,y â Ì ÎŒ t,y (m j,x â Ì ÎŒ t,x ) 2 +(m j,y â Ì ÎŒ t,y ) 2 â m j,x â Ì ÎŒ t,x (m j,x â Ì ÎŒ t,x ) 2 +(m j,y â Ì ÎŒ t,y ) 2 â1 ïŁč ïŁ» .(13.9) It is also common to assume a diagonal covariance for the measurement noise: R t = " Ï 2 r 0 0Ï 2 Ï # , where Ï r is the standard deviation of the range measurement noise and Ï Ï is the standard deviation of the bearing measurement noise. This reflects the assump- tion that range and bearing errors are uncorrelated. 13.6 Monte Carlo Localization (MCL) The second practical realization of Markov localization we consider is Monte Carlo localization (MCL). This algorithm leverages the non-parametric particle filter framework from Chapter 12 and is particularly well suited to global pose localiza- tion and the kidnapped robot problem. 6 6 We can also use MCL to solve the kid- napped robot problem by injecting new randomly sampled particles at each step so that the filter remains sensitive to un- expected measurements. Like the particle filter, MCL represents the belief distribution bel(x t ) by a set of K particles: P t : =x [1] t , x [2] t , ..., x [K] t , where each particle x [k] t represents a hypothesis about the true state x t . Regions of the state space with more particles correspond to higher belief. At each step of the algorithm, we: 1. propagate particles forward using the map-aware state transition model, and 2. weight and resample the particles using the map-aware measurement model. This is summarized in Algorithm 13.4, which is nearly identical to the particle filter algorithm in Algorithm 12.5, the only difference being that the map m now appears in the probabilistic state transition and measurement models. In global localization, MCL typically starts from a broad prior, such as having particles spread over the entire map. As the robot moves and gathers measure- ments, inconsistent hypotheses are down-weighted and vanish during resam- pling, while particles near the true pose accumulate. This makes MCL a pow- erful tool for environments with strong ambiguities, repeated structures, or very uncertain initial conditions. 340 principles of robot autonomy Algorithm 13.4: Monte Carlo Localization Data: P tâ1 , u t , z t , m Result: P t Ì P t =P t =â for k = 1 to K do Sample Ìx [k] t ⌠p(x t | x [k] tâ1 , u t , m) w [k] t = p(z t | Ìx [k] t , m) Ì P t = Ì P t âȘ Ìx [k] t , w [k] t for k = 1 to K do Draw i with probabilityâ w [i] t Add Ìx [i] t toP t returnP t 13.7 Summary In this chapter, we examined the fundamental concepts and algorithms under- lying robot localizationâthe process of estimating a robotâs pose relative to a known map of the environment. We began with a taxonomy of localization problems, distinguishing between pose tracking and global localization, static and dynamic environments, active and passive strategies, and single-robot versus multi-robot settings. This taxonomy provides a conceptual checklist for matching real-world scenarios to appropriate algorithmic tools. Building on the Bayesian estimation framework introduced earlier, we ex- tended the state transition and measurement models to explicitly incorporate map information. This led to the Markov localization algorithm, a general Bayesian filter that serves as a conceptual foundation for map-aware localization. We then explored two important realizations of this framework: EKF localiza- tion, which represents the belief as a unimodal Gaussian and is effective for pose tracking with moderate uncertainty; and MCL, which uses a particle-based, non- parametric representation suitable for multi-hypothesis and global localization problems. We discussed data associationâthe challenge of determining corre- spondences between sensor measurements and known map featuresâand tech- niques such as maximum-likelihood matching and validation gates to improve robustness. Together, these methods form the core of modern localization systems, en- abling robots to estimate their position and orientation using noisy sensors, par- tial maps, and uncertain environments. In later chapters, these ideas will reap- pear in more complex settings such as simultaneous localization and mapping (SLAM), where the robot must estimate both its pose and the map at the same time. robot localization 341 To learn more. For a detailed and foundational treatment of probabilistic localiza- tion, Thrun, Burgard, and Fox [148] remains the canonical reference. The original formulation of the Monte Carlo Localization algorithm is presented by Dellaert et al. [30], while Leonard and Durrant-Whyte [79] provide an early and influ- ential treatment of EKF-based localization with geometric landmarks. Readers interested in recent advances and the connection between localization and SLAM may refer to Cadena et al. [22] and the comprehensive modern text by Carlone et al. [24], which unify localization and mapping under a common probabilistic and optimization-based framework. 13.8 Exercises The starter code for the exercises provided below is available online through GitHub. To get started, download the code by running in a terminal window: git clone https://github.com/StanfordASL/pora-exercises.git We denote Problems requiring hand-written solutions and coding in Python withand, respectively. Problem 1: Extended Kalman Filter Localization In this problem, you will implement an extended Kalman filter (EKF) for robot localization in an environment where the robot can collect relative position mea- surements to a set of four landmarks. We will consider a robot with a discrete- time dynamics model x t+1 = f (x t , u t ) + Δ t defined by: x t+1 = x t + V t cos(Ξ t )ât + Δ x t , y t+1 = y t + V t sin(Ξ t )ât + Δ y t , Ξ t+1 = Ξ t + Ï t ât + Δ Ξ t , and the goal is to estimate the unknown robot pose, x t = [x t , y t , Ξ t ] †. The noise vector Δ t = [Δ x t , Δ y t , Δ Ξ t ] †is a random variable with a zero mean Gaussian distri- bution Δ t ⌠N (0, Q), where Q = 0.1ât 2 I. In this problem, we assume the global ground truth positions of the four landmarks are known. The landmarks are stationary objects in the environment, and their combined state vector is: m = h m 1,x m 1,y m 2,x m 2,y m 3,x m 3,y m 4,x m 4,y i †. As the robot navigates through its environment, it receives noisy measurements of the positions of four landmarks in the environment relative to the robotâs cur- rent pose. The measurement for landmark i is the relative position with the measurement model: z i t = h(x t , i, m) + ÎŽ t = " cos(Ξ t )sin(Ξ t ) â sin(Ξ t )cos(Ξ t ) # " m i,x m i,y # â " x t y t # + ÎŽ t , 342 principles of robot autonomy where the measurements have an associated measurement noise with ÎŽ t âŒN (0, R), where R = 0.25I. The full measurement vector of all landmarks is: z t = ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁ° z 1 t z 2 t z 3 t z 4 t ïŁč ïŁș ïŁș ïŁș ïŁ» . In the file ch13/exercises/ekf _ localization.ipynb, complete the following: 1. Implement the functions robot _ dynamics and robot _ measurement to match the dynamics and measurement models described above. 2. Implement the function dynamics _ jacobian to compute the dynamics Jacobian F t =â x f (x t , u t ). 3. Implement the function measurement _ jacobian to compute the measurement model Jacobian H t = â x h(x t , m) for the model that computes the full mea- surement vector z t . 4. Implement the function ekf _ localization _ update to implement the EKF localization update described in Algorithm 13.2. Note that you wonât need to explicitly have a for loop in this function for each measurement since we have defined the update to be vectorized for all measurements at once. 5. Run the provided code to see how the algorithm performs for the simulated robot. Problem 2: Particle Filter Localization In this problem, we consider the same problem setup defined in Problem 1, where we are localizing a robot given relative position measurements to a set of four known landmarks. In the file ch13/exercises/particle _ filter _ localization. ipynb, complete the following: 1. Implement the functions robot _ dynamics and robot _ measurement to match the dynamics and measurement models described above. Note: you can reuse your solution from the previous problem. 2. Implement the particle filter algorithm update function particle _ filter _ up date as described in Algorithm 13.4. 3. Run the provided code to see how the algorithm performs for the simulated robot. robot localization 343 References [22]C. Cadena et al. âPast, present, and future of simultaneous localization and mapping: Toward the robust-perception ageâ. In: IEEE Transactions on robotics 32.6 (2017), p. 1309â1332. [24]L. Carlone et al., eds. SLAM Handbook. From Localization and Mapping to Spatial Intelligence. Cambridge University Press, 2026. [30]F. Dellaert et al. âMonte carlo localization for mobile robotsâ. In: Proceed- ings 1999 IEEE international conference on robotics and automation (Cat. No. 99CH36288C). Vol. 2. IEEE. 1999, p. 1322â1328. [79]J. J. Leonard and H. F. Durrant-Whyte. âSimultaneous map building and localization for an autonomous mobile robot.â In: IROS. Vol. 3. 1991, p. 1442â1447. [148]S. Thrun, W. Burgard, and D. Fox. Probabilistic Robotics. MIT Press, 2005. 14 Simultaneous Localization and Mapping (SLAM) In Chapter 13, we studied robot localization under the assumption that a map of the environment, m, was known. While this assumption simplifies the estimation process, it is rarely met in practice. Robots often operate in a priori unknown or partially known environments. Examples include autonomous search-and-rescue in collapsed buildings, planetary exploration, and mapping of underwater struc- tures. In such cases, a robot must concurrently infer its own state and build a map from noisy sensor data. This joint estimation problem is known as simultaneous localization and mapping (SLAM). The term SLAM and many classical algorithms were popularized by Thrun, Burgard, and Fox [148, Chs. 10â13], which remains a standard reference. SLAM plays a central role in the perception and navigation layers of an au- tonomy stack. By providing a consistent, evolving spatial frame of reference, it allows downstream planning, control, and decision-making components to oper- ate effectively in previously unseen environments. The SLAM problem emerged in the late 1980s, with seminal contributions by Hugh Durrant-Whyte, John Leonard, and others, who framed it as a probabilis- tic joint estimation of pose and map. Early approaches treated SLAM as a large filtering problem. The EKF 1 provided a conceptually elegant way to maintain a 1 R. Smith, M. Self, and P. Cheeseman. âEstimating uncertain spatial relation- ships in roboticsâ. In: Autonomous robot vehicles. Springer, 1990, p. 167â193, J. J. Leonard and H. F. Durrant-Whyte. âSi- multaneous map building and localiza- tion for an autonomous mobile robot.â In: IROS. vol. 3. 1991, p. 1442â1447 joint Gaussian distribution over all robot poses and landmarks. In this formula- tion, the state vector contains the robot pose together with the positions of all N landmarks, and the EKF maintains a (3 + 2N)Ă (3 + 2N) covariance matrix that encodes correlations between every pair of variables. As the map grows, stor- ing and updating this dense covariance incursO(N 2 ) memory and computation, which quickly becomes prohibitive for large-scale mapping. Moreover, repeated linearization of nonlinear motion and measurement models around a single mean can introduce inconsistency and overly optimistic uncertainty estimates. In the early 2000s, particle-filter methods, most notably FastSLAM 2 offered a 2 M. Montemerlo et al. âFastSLAM: A factored solution to the simultaneous lo- calization and mapping problemâ. In: Aaai/iaai 593598.2 (2002), p. 593â598 different perspective. By combining a particle-based representation of the robotâs trajectory with separate Gaussian estimates for individual landmarks (a technique known as RaoâBlackwellization), FastSLAM improved scalability and handled data association more flexibly. Here, data association refers to the problem of determin- ing which previously mapped landmark, if any, corresponds to each new sensor This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 346 principles of robot autonomy measurement, an ambiguity that arises when multiple landmarks look similar or when sensing is noisy. At the same time, particle-filter approaches are suscep- tible to particle depletion. Over long trajectories, repeated weighting and resam- pling cause most particles to carry negligible weight, so that only a few distinct hypotheses effectively remain. When this happens, the particle set no longer provides a good approximation of the posterior, and the algorithm may become brittle unless many particles are used or more sophisticated proposal distribu- tions are designed. Subsequent work in the mid-2000s adopted an optimization-based view. Rather than updating a filter one step at a time, researchers formulated SLAM as a sparse nonlinear least-squares problem over a pose graph. In this representation, nodes correspond to robot poses and edges represent relative pose constraints obtained from odometry, loop closures, or scan matching. This graph-based SLAM per- spective made it possible to exploit sparsity in the underlying problem, leverage robust cost functions to manage outliers, and apply incremental solvers for real- time operation 3 . The factor-graph formalism further generalizes this idea: both 3 Representative systems include early pose-graph formulations by Lu and Mil- ios [89], as well as modern optimiza- tion and incremental smoothing ap- proaches such as square-root SAM [29], iSAM [62], and g2o [73]. robot poses and map variables are represented as nodes, while each measure- ment or prior contributes a factor, that is, a local term in the joint probability density that depends only on a small subset of variables. Factor graphs provide a unified way to encode heterogeneous constraints (from odometry to landmark observations and calibration parameters) in a single probabilistic model and now underpin most modern SLAM back-ends 4 . 4 F. Dellaert.âFactor graphs and GT- SAM: A hands-on introductionâ.In: Georgia Institute of Technology, Tech. Rep 2.4 (2012), F. Dellaert. âFactor graphs: Exploiting structure in roboticsâ. In: An- nual Review of Control, Robotics, and Au- tonomous Systems 4.1 (2021), p. 141â166 Recent years have seen SLAM move beyond purely geometric mapping. Visual and visualâinertial SLAM, dense 3D reconstruction, semantic mapping, neural implicit representations (e.g., NeRF and Gaussian splatting), and learning-based front-ends all build on the same probabilistic foundations while extending the range of environments and tasks for which SLAM is viable. Modern SLAM sys- tems combine probabilistic estimation, large-scale optimization, and learned rep- resentations, and are increasingly viewed as core components of broader âspatial AIâ systems. In the remainder of this chapter, we develop a unified view of SLAM that con- nects these ideas. Section 14.1 introduces the main algorithmic paradigms for SLAM, distinguishing filter-based and smoothing-based back-ends and clarify- ing the role each plays in modern systems. Next, we turn to the SLAM front-end in Section 14.2, describing how raw sensor data are converted into constraints, and then to modality-specific pipelines in Section 14.3, where we illustrate rep- resentative designs for visual, lidar, and radar SLAM. Building on this intuition, Section 14.4 formalizes SLAM as a Bayesian state estimation problem, introduces the underlying state-space models for motion and measurement, and derives the online and full SLAM formulations. The following sections instantiate this frame- work in concrete algorithms: EKF-SLAM in Section 14.5, particle-filter-based methods such as FastSLAM in Section 14.6, and optimization-based approaches including pose-graph SLAM and factor-graph SLAM in Sections 14.7 and 14.8. Finally, in Section 14.9 we survey advanced and emerging methods, dense and se- simultaneous localization and mapping (slam) 347 mantic mapping, neural implicit representations, and learning-based front-ends, and connect them to the broader vision of spatial AI. 14.1 SLAM Paradigms Modern SLAM systems are typically organized into two conceptual layers. The front-end processes raw sensor data and extracts a set of constraints between states and map elements: features, correspondences, relative poses, and loop closures. Loop closures occur when the robot revisits a previously seen place and obtains a measurement that links two non-consecutive poses 5 , and these constraints are 5 For example, recognizing a corridor or room visited much earlier. crucial for correcting accumulated drift. The back-end takes all these constraints and solves the underlying estimation problem, either recursively (filtering) or by optimizing over a window or the full trajectory (smoothing). In this section, we focus on the back-end and distinguish two main paradigms: filter-based approaches and smoothing-based approaches. Both are grounded in the same probabilistic models introduced later in Section 14.4, but they make different choices about which variables to estimate explicitly and how to use past measurements. 14.1.1 Filter-based Approaches Filter-based SLAM maintains a compact state estimate that is updated online as new sensor measurements arrive. The key idea is to propagate a belief over the current robot state and map using a recursive Bayes filter, without explicitly revisiting all past data at each step. In EKF-SLAM, the robot state and map are stacked into a single augmented state vector, and the joint belief is modeled as a multivariate Gaussian. Mo- tion and measurement models are linearized around the current estimate, and the EKF predictionâcorrection equations are applied at every time step (see Sec- tion 12.3.1 for the underlying EKF machinery). This yields an online algorithm that fuses information incrementally, but the covariance matrix is dense and of size (n + 2N)Ă (n + 2N) for N landmarks and n robot state variables. Storing and updating this matrix incurs O(N 2 ) memory and computational cost, which becomes prohibitive as the map grows. Moreover, because non-linear motion and measurement models are repeatedly linearized around a single mean, lin- earization errors can accumulate over time and lead to inconsistent uncertainty estimates (see, for example, Thrun, Burgard, and Fox [148, Chs. 10â13]). RaoâBlackwellized particle filters, such as FastSLAM 6 , adopt a hybrid strategy. A 6 M. Montemerlo et al. âFastSLAM: A factored solution to the simultaneous lo- calization and mapping problemâ. In: Aaai/iaai 593598.2 (2002), p. 593â598 particle filter represents the distribution over robot trajectories, while each par- ticle carries separate Gaussian estimates for the landmarks. Conditioned on a sampled trajectory, the landmark estimates become conditionally independent, so that the map can be updated by running a set of tractable Kalman filters, specifically one per landmark, instead of one huge joint filter. This factorization dramatically reduces the cost per update and makes it easier to handle ambigu- 348 principles of robot autonomy ous data association. At the same time, particle-filter-based methods introduce new practical challenges, such as designing good proposal distributions, avoid- ing particle depletion over long trajectories, and maintaining global consistency. A detailed treatment of FastSLAM and RaoâBlackwellized particle filters can be found in Thrun et al. [148, Ch. 13]. Filter-based methods are appealing when computation and memory budgets are tight, and when online operation with bounded per-step cost is a primary requirement. However, when long-term consistency, large-scale mapping, and aggressive loop closing are critical, they may struggle to fully exploit all avail- able measurements, especially those that create strong constraints between dis- tant poses. 14.1.2 Smoothing-based Approaches Smoothing-based SLAM treats the entire robot trajectory and map as variables in a global estimation problem. Rather than maintaining only the current poste- rior p(y t | z 1:t , u 1:t ), smoothing methods aim to recover either (i) the full posterior p(y 1:t | z 1:t , u 1:t ) or (i) a maximum a posteriori (MAP) estimate of the entire tra- jectory and map. A convenient way to express this is through a graph representation with nodes and edges/factors. âą Nodes represent unknown variables, such as robot poses at different times and landmark positions. âą Edges or factors represent measurements or priors that relate a small subset of these variables: odometry constraints between consecutive poses, loop-closure constraints between non-consecutive poses, landmark observations that couple a pose to a landmark, and prior terms that anchor the map. In a pose graph, nodes are pose variables and edges are relative pose constraints (odometry and loop closures). In a more general factor graph, both poses and map variables are nodes, and each measurement contributes a factor 7 . 7 A local term in the joint probability density that depends only on the vari- ables involved in that measurement. From a probabilistic standpoint, each factor corresponds to a likelihood or prior term, and the overall objective (for MAP estimation) is to find the trajectory and map that best satisfy all factors simultaneously. Under Gaussian assump- tions, this leads to a sparse nonlinear least-squares problem. The sparsity arises because each measurement involves only a few variables 8 , so the correspond- 8 For example, a single relative pose con- straint involves two poses, and a land- mark observation involves one pose and one landmark. ing Jacobian and Hessian matrices contain many zeros. Modern sparse solvers exploit this structure to solve very large SLAM problems efficiently. References for graph- and factor-graph-based SLAM include Dellaert and Kaess [29], Kaess, Ranganathan, and Dellaert [62], and KĂŒmmerle et al. [73]. Smoothing has two main advantages over pure filtering: 1. It can re-linearize constraints in light of new data. When new measurements arrive, such as from a loop closure, the entire trajectory and map can be re- simultaneous localization and mapping (slam) 349 optimized, improving consistency relative to filters that only update the cur- rent state. 2. It naturally incorporates loop closures by explicitly adding constraints between non-consecutive poses and adjusting the entire trajectory to satisfy them. This allows accumulated drift to be redistributed along the path when the robot revisits known areas. 14.1.3 Choosing a Paradigm In practice, the choice between filtering and smoothing depends on several fac- tors: âą Real-time constraints. If strict real-time updates with tightly bounded per-step latency are required and computational resources are limited, filter-based ap- proaches (EKF-SLAM, FastSLAM) are attractive. âą Problem scale and loop closures. For large environments with many loop clo- sures and long missions, smoothing- based methods and pose- or factor-graph formulations tend to yield more accurate and consistent maps, since they can re-optimize the full trajectory when new constraints arrive. âą Implementation complexity. Filters are conceptually straightforward and often easier to implement for small systems. Graph-based methods require addi- tional mathematical and software infrastructure 9 , but they are highly modular 9 For example, sparse linear algebra and nonlinear optimization libraries. and extensible once in place. âą Application priorities. Short-term navigation may prioritize fast local odometry and modest map maintenance, while long-term mapping, multi-session oper- ation, and multi-robot scenarios benefit from the global consistency offered by smoothing and graph optimization. In modern systems, the boundary between these paradigms is increasingly blurred. Incremental smoothing methods, such as iSAM 10 , provide real-time up- 10 M. Kaess, A. Ranganathan, and F. Del- laert.âiSAM: Incremental smoothing and mappingâ. In: IEEE Transactions on Robotics 24.6 (2008), p. 1365â1378 dates while retaining many of the advantages of batch optimization, and are widely used as SLAM back-ends in contemporary robotics libraries. 14.2 Front-End So far, we have focused on back-end formulations: how to structure and solve the estimation problem once constraints are given. In a SLAM system, however, those constraints do not appear magically. They are distilled from raw sensor data by the front-end, which transforms pixels, point clouds, and other measurements into discrete relationshipsâsuch as relative poses, landmark observations, and loop closuresâthat the back-end subsequently enforces. 350 principles of robot autonomy A useful way to think about this division of labor is the following. The back- end is the inference engine that reasons about the history of the robotâs mo- tion and the structure of the environment, while the front-end is the perceptual pipeline that decides what information the back-end sees and how it is packaged. A strong back-end cannot compensate for a front-end that produces systematic outliers or weak, uninformative constraints. At a high level, most SLAM front-ends can be understood as variants of the same conceptual pipeline: 1. Feature extraction. Identify salient structures in the sensor data, such as key- points, edges, geometric primitives, and learned descriptors. 2. Data association. Decide which current features correspond to which previously seen features or map elements. 3. Outlier rejection. Detect and remove inconsistent or spurious associations before they contaminate the estimate. 4. Loop closure detection. Recognize previously visited places in order to add long- range constraints that correct drift. 5. Scan alignment and registration. For range data, estimate relative poses between overlapping point clouds. The detailed implementation of each stage depends strongly on the sensing modality 11 , but their role in the SLAM system is shared: they determine which 11 For example, cameras, lidar, radar, or others. parts of the environment become landmarks or edges in the graph, how confi- dently we relate different poses, and when we introduce powerful loop-closure constraints. In the rest of this subsection, we briefly revisit these stages, connect- ing them to the perception tools developed in earlier chapters and emphasizing their global impact on SLAM. 14.2.1 Feature Extraction From the SLAM perspective, feature extraction is the step where we decide which aspects of the environment will serve as âhandlesâ for localization and mapping. This builds directly on the classical perception techniques of Chapter 8, which developed image filtering, edge and corner detection, descriptors, and geometric feature extraction, as well as the point cloud registration tools of Section 8.2.2. 12 12 Deep learning architectures for per- ception, introduced in Chapter 9, pro- videlearnedalternativestohand- designed features and are increasingly used in modern SLAM front-ends. In visual SLAM, the front-end typically detects and describes repeatable image featuresâfor example corners, blobs, or local patchesâusing classical methods such as Harris corners and SIFT-like descriptors, or modern learned features. These features are chosen to be robust to viewpoint and illumination changes so that they can be matched reliably across time and across cameras. The re- sulting 2D keypoints and descriptors form the basis for constructing geometric constraintsâsuch as epipolar relations and reprojection errorsâbetween camera poses and 3D landmarks, as discussed in Chapter 6 and Chapter 7. simultaneous localization and mapping (slam) 351 In lidar-based SLAM, the analogous role is played by geometric primitives in point clouds: planes, edges, corner-like structures, or local surface patches. Ex- tracting these primitives 13 provides stable anchor points that can be tracked 13 For instance, using segmentation and model fitting methods from Chapter 8. across scans and used for scan-to-scan or scan-to-map registration. For radar and other modalities, feature extraction must cope with lower res- olution, clutter, and multipath. A common representation for automotive radars is the rangeâDoppler image: a 2D array obtained by applying Fourier transforms to the raw radar chirps, whose horizontal axis indexes range bins (distance from the sensor) and whose vertical axis indexes Doppler bins (radial velocity). Each pixel intensity reflects the strength of the return from targets at a given distance and relative speed, so a rangeâDoppler image can be viewed as a grayscale im- age that encodes both where objects are and how fast they are moving. In this representation, hand-crafted geometric cues are often combined with learned rep- resentations 14 to produce robust, repeatable signatures that can be matched over 14 For example, CNNs operating directly on rangeâDoppler images. time. 14.2.2 Data Association Once features have been extracted, the next crucial step is data association: deter- mining whether a feature observed at time t corresponds to a previously observed feature or map element. This is the mechanism by which we translate âthis cor- ner in the current imageâ into âthe same corner we saw two seconds agoâ or âlandmark m j in the mapâ. As in the localization setting in Chapter 13, data association is challenging: perceptual aliasing, sensor noise, occlusions, and environmental changes all make different places look similar, and the same place can look different over time. A correct association introduces a valid constraint between poses and landmarks, whereas an incorrect association can inject a large, systematically biased con- straint that corrupts the entire estimate. In practice, SLAM systems use a combination of techniques for data associa- tion, including: âą Descriptor similarity, such as the distance in feature space for visual keypoints. âą Geometric predictions from the current state estimate, such as projecting a landmark into the image or into a scan. âą Probabilistic gating, where candidate matches are accepted only if they lie within an uncertainty-dependent region (often using Mahalanobis distance). âą Higher-level cues, such as semantic labels like whether a feature belongs to a âdoorâ object, or temporal consistency. In many systems, data association is the most fragile component, but designing strategies with high recall (few missed true matches) and high precision (few false matches) is nonetheless critical for robust operation. 352 principles of robot autonomy 14.2.3 Outlier Rejection Even with careful association, some matches will be wrong. The front-end must therefore include explicit mechanisms for identifying and rejecting outliers before they reach the back-end. This mirrors the geometric verification and model fitting pipelines discussed in Chapter 8 15 , but now the models are camera geometry, 15 For example, RANSAC for line fitting or object pose estimation. relative poses, or scan alignment. Common strategies for outlier rejection include: âą Geometric consistency checks, such as verifying that matched features satisfy epipolar constraints or that 3D-to-2D correspondences are compatible with a plausible camera pose. âą Robust model fitting, for example using RANSAC and its variants to infer a relative pose or homography from putative matches while discarding outliers. âą Statistical tests on residuals, where measurements whose errors are inconsistent with the expected covariance are discarded or downweighted. âą Consistency with the current map, for instance requiring that new measurements be compatible with already-estimated landmarks and surfaces. From the standpoint of the SLAM back-end, outlier rejection is not an optional cleanup step: a handful of large, unmodeled outliers can easily overwhelm even a sophisticated optimization or filtering algorithm. 14.2.4 Loop Closure Detection A defining capability of SLAM systems is loop closure detection. Loop closure detection involves recognizing that the robot has returned to a previously visited place and generating a constraint that links the corresponding poses. From a global perspective, loop closures are the main tool for correcting accumulated drift: they tie together distant parts of the trajectory and allow the back-end to redistribute errors along the path. Conceptually, loop closure detection extends data association from individual features to whole places. The front-end must answer questions like âdoes the current view correspond to some earlier pose x k ?â and, if so, âwhat is the relative transformation between x t and x k ?â. Typical loop closure pipelines involve the following steps: 1. Constructing a compact descriptor for each place. For example, a bag-of-words representation over visual features, a global descriptor for a lidar scan, or a learned embedding from a neural network. 2. Using this descriptor to retrieve a small set of candidate earlier poses that are likely to correspond to the same place. 3. Verifying these candidates geometrically, such as by estimating a relative pose and checking the consistency of reprojection or registration errors. simultaneous localization and mapping (slam) 353 Visual place-recognition methods (both classical and learned) and lidar scan descriptors (such as scan-context-style approaches) implement these ideas with different design choices and trade-offs between robustness, invariance, and com- putational cost. Because each accepted loop closure introduces a long-range constraint in the graph, the stakes are high: a true loop closure can dramatically improve global consistency, whereas a false one can severely distort the map. For this reason, loop closure modules are often conservative, preferring to miss some potential closures rather than accept unreliable ones. 14.2.5 Scan Alignment and Iterative Closest Point (ICP) In range-based SLAM, an additional central task is scan alignment: estimating the relative pose between overlapping point clouds or depth images. This aligns successive lidar sweeps, RGB-D frames, or accumulated submaps and provides relative pose constraints to the back-end. Section 8.2.2 introduced the point cloud registration problem and the Iterative Closest Point (ICP) algorithm in detail. In the SLAM front-end, ICP and its many variants are used as a building block: âą Given an initial guess for the relative pose between two scans (from odometry, IMU integration, or feature-based matching), ICP refines this guess by itera- tively pairing points or primitives and solving for the rigid transformation that best aligns them. âą The resulting transformation and its estimated uncertainty are then passed to the back-end as a relative pose constraint between the corresponding robot poses. While conceptually simple and powerful, ICP is sensitive to initialization, can converge to local minima, and can be affected by outliers or partial overlap be- tween scans. Modern SLAM systems therefore rarely rely on ICP alone: feature- based methods or learned global descriptors are used to obtain robust initial esti- mates and to reject poor matches, with ICP providing fine geometric refinement within a multi-stage registration pipeline. 14.2.6 Summary Putting these pieces together, the front-end can be viewed as a modular pipeline that starts from raw sensor streams and outputs a set of carefully curated con- straints: local feature-based measurements, loop-closure links, and scan-to-scan registrations, each with an associated uncertainty. In Section 14.3, we will see how this conceptual pipeline adapts to different sensing modalities, and in Section 14.4 we will formalize how these constraints enter the probabilistic SLAM model and drive the back-end algorithms developed in the remainder of the chapter. 354 principles of robot autonomy 14.3 SLAM Across Sensing Modalities Up to this point, we have separated the SLAM problem into a front-end, which converts raw sensor observations into constraints, and a back-end, which estimates the trajectory and map given those constraints. In practice, the design of both components is strongly shaped by the available sensors. Cameras, lidar, and radar provide very different raw data, and thus require different feature extraction, data association, and loop-closure strategies, even though they ultimately feed the same types of constraints into the back-end. In this section, we illustrate how the general front-end pipeline from Sec- tion 14.2 (feature extraction, data association, outlier rejection, loop closure, and scan alignment) specializes to three widely used sensing modalities: cameras (vi- sual SLAM), lidar (lidar SLAM), and radar (radar SLAM). Our goal is not to cover each modality exhaustively, but to highlight how the same probabilistic ideas manifest in different front-end designs and what this implies for the back-end. 14.3.1 Vision-Based SLAM Vision-based SLAM uses cameras as the primary source of information. Common configurations include monocular, stereo, and visualâinertial 16 setups. The raw 16 Visualâinertial setups combine cam- eras with IMUs in a complementary way. input is a stream of images that the front-end must transform into geometric constraints between camera poses and 3D structure. Front-end. Building on the feature extraction and multi-view geometry tools from Chapter 6âChapter 8, a typical visual front-end: âą Detects and describes repeatable 2D features, such as corners, blobs, or local patches, using classical descriptors or learned features. âą Matches features across frames to obtain 2Dâ2D or 2Dâ3D correspondences. âą Estimates relative camera motion using geometric relations such as the essential matrix and the Perspective-n-Point (PnP) model. âą Performs outlier rejection, for example with RANSAC on the essential matrix or PnP residuals, and passes only geometrically consistent constraints to the back-end. âą Runs place-recognition and geometric verification to propose and confirm loop closures. Recall from Chapter 7 that, for calibrated cameras, the essential matrix E = [t] Ă R encodes the epipolar constraint between two views: p †E p âČ = 0 for cor- responding normalized image points p, p âČ . Estimating E from feature matches allows recovery of the relative rotation R and translation direction t between frames, up to scale. Similarly, the PnP problem (see Chapter 7) recovers the cam- era pose that best explains a set of 2Dâ3D correspondences between image points simultaneous localization and mapping (slam) 355 and map landmarks. In a SLAM front-end, these tools provide the relative pose edges and reprojection constraints that will later appear in the pose or factor graph. For monocular cameras, the front-end must handle inherent scale ambiguity: relative motion can be recovered only up to an unknown scale until additional information resolves it 17 . Stereo and visualâinertial setups reduce or remove this 17 For example, from motion parallax, priors, or other sensors. ambiguity by providing direct depth estimates or inertial motion constraints. Back-end. Visual SLAM back-ends commonly use sparse bundle adjustment or pose-graph optimization: frame-to-frame relative pose estimates, landmark re- projection errors, and inertial constraints are assembled into a single optimization problem. Systems such as PTAM 18 , ORB-SLAM 19 , and VINS-Mono 20 follow this 18 G. Klein and D. Murray.âParal- lel tracking and mapping for small AR workspacesâ. In: 2007 6th IEEE and ACM international symposium on mixed and aug- mented reality. IEEE. 2007, p. 225â234 19 R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos.âORB-SLAM: A versa- tile and accurate monocular SLAM sys- temâ. In: IEEE transactions on robotics 31.5 (2015), p. 1147â1163 20 T. Qin, P. Li, and S. Shen. âVins-mono: A robust and versatile monocular visual- inertial state estimatorâ. In: IEEE trans- actions on robotics 34.4 (2018), p. 1004â 1020 design: a front-end that maintains a set of keyframes, tracks features, and pro- poses loop closures, coupled with a back-end that optimizes over camera poses (and possibly landmark positions) using the factor-graph machinery discussed in Section 14.1. Perspective. Vision-based SLAM excels at capturing rich appearance information and can be implemented with low-cost hardware, but it is sensitive to lighting changes, motion blur, and textureless scenes. These weaknesses motivate the use of robust front-end design and additional sensors such as IMUs and lidar. 14.3.2 Lidar-Based SLAM Lidar sensors provide dense or semi-dense 3D point clouds that directly encode geometry. They are widely used in autonomous driving and field robotics, where accurate metric localization is required over large areas and in diverse conditions. Front-end. A typical lidar SLAM front-end includes: âą Scan preprocessing, including deskewing and motion compensation, filtering, and ground removal. âą Feature extraction, identifying edge-like or planar structures or small surface patches in the point cloud. âą Scan alignment, using ICP or feature-based registration (building on the point cloud registration methods introduced in Section 8.2.2). âą Loop closure detection, often via global scan descriptors or learned embeddings that summarize the shape of a scan or local map. ICP-based alignment provides relative pose constraints between consecutive scans (scan-to-scan) or between a scan and an accumulated submap (scan-to- map). Combining these with loop-closure constraints yields a rich set of geomet- ric relationships between poses that the back-end can exploit. 356 principles of robot autonomy Back-end. On the back-end, pose-graph optimization is standard: nodes repre- sent vehicle poses, and edges encode relative poses from scan registration and loop closures. Because lidar provides high-accuracy range data, even small mis- alignments can accumulate into significant drift if not corrected. Robust cost functions and outlier-resistant optimization strategies are therefore crucial. In- cremental solvers allow these graphs to be updated in real time as new scans arrive. Classic systems such as LOAM (lidar Odometry and Mapping) 21 demonstrate 21 J. Zhang, S. Singh, et al. âLOAM: Lidar odometry and mapping in real-time.â In: Robotics: Science and systems. Vol. 2. 9. Berkeley, CA. 2014, p. 1â9 how a carefully designed lidar front-end, coupled with a graph-based back-end, can achieve centimeter-level accuracy in real time. Perspective. Compared to cameras, lidar front-ends work with sparser but met- rically precise data. They avoid some of the perceptual aliasing issues of vision, but introduce their own challenges in scan registration, handling dynamic objects, and coping with adverse weather or sensor artifacts. 14.3.3 Radar SLAM Radar sensors, historically used in autonomy mainly for collision avoidance, are increasingly used for SLAM due to their robustness to fog, rain, dust, and poor lighting, and their long-range detection capabilities. Front-end. Radar returns are noisy, have lower angular resolution than cameras or lidar, and are degraded by multipath effects. As a result, feature extraction and data association are more challenging. Typical radar front-ends: âą Compute rangeâDoppler or rangeâangle images from raw radar returns. âą Extract prominent reflectors or local patterns as features, often using filtering and thresholding techniques adapted from Chapter 8. âą Build global descriptors of scans or trajectories, or use learned embeddings tailored to radar data, to support place recognition and loop closure detection. âą Estimate relative motions using registration in rangeâDoppler space, or by aligning reconstructed point clouds (when available), often in combination with inertial data. Because of the high false-positive rate and spurious reflections, strong outlier rejection and conservative loop closure validation are especially important. Back-end. Back-end formulations for radar SLAM often mirror those of lidar SLAM: pose graphs whose edges come from radar-based relative pose estimates and loop closures. Recent systems frequently combine radar with other modal- ities, such as radarâinertial or radarâlidar odometry, to compensate for radarâs lower spatial resolution and to operate reliably in conditions where cameras and lidar are degraded. simultaneous localization and mapping (slam) 357 Perspective. Radar front-ends emphasize robustness over raw information den- sity. When fused with other modalities, radar can significantly improve reliability in adverse conditions, which is increasingly important in safety-critical applica- tions. 14.3.4 Discussion Each sensing modality offers a different balance between richness of data, robust- ness, and environmental adaptability. Cameras provide dense appearance cues but can fail under challenging lighting or in texture-poor scenes. Lidar yields precise geometry but is more expensive and can be affected by adverse weather. Radar offers robustness and long range at the cost of sparser, noisier measure- ments and more difficult data association. Modern SLAM systems therefore rarely rely on a single sensor. Instead, they combine complementary modalitiesâfor example, visualâinertial odometry with lidar, or lidar with radarâso that the strengths of one sensor compensate for the weaknesses of another. This trend underscores the importance of the modular front-end and back-end architecture developed in Sections 14.1 and 14.2: front- ends can be adapted or extended as sensors change, while back-ends operate on a common abstraction of constraints. In the next section, Section 14.4, we temporarily abstract away these modality- specific details and formalize SLAM as a Bayesian state estimation problem. There, we introduce the motion and measurement models that underlie both filtering and smoothing formulations, and show how the constraints produced by the various front-ends enter the probabilistic SLAM framework. 14.4 Mathematical Foundations of SLAM Equipped with an understanding of the front-end, we can now formalize the SLAM problem as a Bayesian state estimation problem. This formal viewpoint captures, in a single probabilistic model, how the robot state evolves over time and how the map of the environment is refined as new data arrive. It also pro- vides the bridge between the front-end constraints and the filtering- and smoothing- based back-ends discussed in Section 14.1. Throughout this section we assume that the map m is static over the time hori- zon of interest: walls do not move, landmarks remain fixed, and the environment does not change in ways that must be explicitly modeled. This assumption is appropriate for many indoor and urban scenarios and keeps the notation man- ageable. Extensions to dynamic mapsâfor example, modeling moving objects or slowly changing geometryâtypically augment the state with additional vari- ables for dynamic entities and introduce explicit time-dependence in the map; we briefly return to these ideas in later chapters. Formally, given a sequence of control inputs u 1:t and sensor measurements z 1:t , the SLAM problem asks the robot to estimate both its trajectory x 1:t (or at least 358 principles of robot autonomy its current state x t ) and the map m of the environment. As in Chapter 13, we treat x t â R n as the robot state at time t, typically including pose and possibly velocity or other motion-related variables. The map m encodes properties of the environment according to a chosen representation. We reuse the same families of map models introduced in Chapter 13, namely feature-based, dense/grid-based, and hybrid maps, which can include sparse sets of landmarks, occupancy grids or signed distance fields, and combinations thereof for systems that require both long-range localization and detailed local geometry. Since the goal is to estimate both the robot state and the map, it is convenient to combine them into a single joint state at time t: y t : = ( x t , m ) , and we denote the joint state trajectory by: y 1:t : = ( y 1 , . . . , y t ) . Two closely related formulations are particularly important in practice. Definition 14.4.1 (Online SLAM.). The goal of online SLAM is to estimate the current robot state together with the map. Mathematically, this corresponds to the belief: bel(y t ) = p(y t | z 1:t , u 1:t ).(14.1) Definition 14.4.2 (Full SLAM.). The goal of full SLAM is to estimate the entire trajectory of the robot together with the map. Mathematically, this corresponds to the belief: bel(y 1:t ) = p(y 1:t | z 1:t , u 1:t ).(14.2) The distinction between these two viewpoints is illustrated in Section 14.4. Online SLAM focuses on the most recent pose and the map, which is often what is needed for real-time control and navigation. Full SLAM retains the entire pose history, which is particularly useful for building globally consistent maps, enforcing loop closures, or performing offline analysis over long missions. In both cases, localization and mapping are tightly coupled: accurate local- ization requires an accurate map, and accurate mapping depends on reliable lo- calization. This interplay makes SLAM sensitive to drift, data association errors, and outliersâissues that modern algorithms address via loop closure detection, robust front-ends, and global optimization in the back-end. 14.4.1 Motion and Measurement Models To connect controls, states, and measurements in a probabilistic framework, we must first specify models for how the robot moves and how its sensors behave. At each time step t, the robot receives: âą A control input u t , such as wheel velocities, steering commands, thrust forces. simultaneous localization and mapping (slam) 359 Figure 14.1: Online SLAM prob- lems estimate only the current robot state together with the map, whereas full SLAM prob- lems estimate the entire trajec- tory of past robot states (the state history) along with the map. âą Sensor observations z t , such as lidar scans, images, rangeâbearing measure- ments. The full histories up to time t are denoted by u 1:t = ( u 1 , . . . , u t ) and z 1:t = ( z 1 , . . . , z t ) . The robotâs motion is captured by a state transition model: x t+1 = f (x t , u t ) + Δ t , where f (·) encodes the deterministic dynamics and Δ t is stochastic process noise with distribution p(Δ t ). The term Δ t accounts for unmodeled effects such as slippage, disturbances, or modeling errors. Sensor observations are modeled by a measurement function: z t = h(x t , m) + ÎŽ t , where h(·) maps the robot state and map to an ideal, noise-free measurement, and ÎŽ t is measurement noise with distribution p(ÎŽ t ). This noise captures sensor imperfections and environmental effects, such as reflections, lighting changes, and occlusions. Both motion and measurement models may be linear or nonlinear, depending on the sensing and actuation setup. A few simple examples illustrate this variety: Example 14.4.1 (Linear motion model (odometry).). For a differential-drive robot with small wheel slippage, the dynamics can be approximated as: x t+1 = x t + Bu t + Δ t , where B is a constant matrix mapping wheel velocities u t to pose increments. Example 14.4.2 (Nonlinear motion model (unicycle).). A more realistic planar model uses the robotâs heading: x t+1 = x t + V t cos(Ξ t )ât + w x t , y t+1 = y t + V t sin(Ξ t )ât + w y t , Ξ t+1 = Ξ t + Ï t ât + w Ξ t , where V t and Ï t are commanded linear and angular velocities and w x t , w y t , w Ξ t represent process noise. 360 principles of robot autonomy Example 14.4.3 (Linear measurement model (1D range).). If a robot moves along a line and measures the distance to a fixed landmark at position m, then: z t = mâ x t + v t , which is linear in x t . Example 14.4.4 (Nonlinear measurement model (rangeâbearing in 2D).). For a landmark at position m x , m y , the sensor might return range and bearing: r t = q (m x â x t ) 2 + (m y â y t ) 2 + ÎŽ r t , Ï t = atan2(m y â y t , m x â x t )â Ξ t + ÎŽ Ï t , which is nonlinear in the robot pose and landmark coordinates. Example 14.4.5 (Nonlinear measurement model (camera projection).). A 3D land- mark ( X, Y, Z ) projects to image coordinates via: " u v # = 1 Z " f x 0c x 0f y c y # ïŁź ïŁŻ ïŁ° X Y Z ïŁč ïŁș ïŁ» + ÎŽ t , where f x , f y are focal lengths and c x , c y the principal point. The division by Z makes the model nonlinear. These models, together with the choice of map representation, fully specify the probabilistic SLAM problem. 14.4.2 Bayesian Formulation of SLAM From a Bayesian perspective, SLAM is a problem of inferring unknown quantities (the trajectory and map) from known data (controls and measurements), given probabilistic models for motion and sensing. Using Bayesâ rule, the full SLAM posterior can be written as: p(y 1:t | z 1:t , u 1:t )â p(z 1:t | y 1:t , u 1:t ) p(y 1:t | u 1:t ),(14.3) where: âą p(z 1:t | y 1:t , u 1:t ) is the likelihood, which encodes how probable the sensor data are given a particular trajectory and map; and âą p(y 1:t | u 1:t ) is the prior, which captures knowledge about how the system evolves under the controls, as well as any prior assumptions on the map. The distinction between online and full SLAM discussed above is reflected in where we place the focus: online SLAM is concerned with the marginal p(y t | z 1:t , u 1:t ), obtained by integrating out past states from Equation (14.3), whereas full SLAM keeps the entire path p(y 1:t | z 1:t , u 1:t ). In the remainder of the chapter, simultaneous localization and mapping (slam) 361 we start from this Bayesian formulation and derive classical solutions: EKF-SLAM for online SLAM and FastSLAM for full SLAM. We then move on to modern graph-based approaches, robust optimization, and registration methods such as ICP and bundle adjustment, which can be interpreted as optimization-based ap- proximations to the same underlying posterior. 14.4.3 State-space Factorization for SLAM Direct computation of the SLAM posterior is intractable in all but the simplest cases. However, the problem becomes manageable once we exploit conditional independencies implied by the motion and measurement models. The key assumption is the Markov property of the dynamics: the future de- pends on the past only through the present state. For the motion model, this gives: p(x t+1 | x 1:t , m, u 1:t ) = p(x t+1 | x t , u t ), which states that the next state depends only on the current state and the latest control. Similarly, for the measurement model we assume: p(z t | x 1:t , m, z 1:tâ1 , u 1:t ) = p(z t | x t , m), meaning that the current measurement depends only on the current state and the map, not on the full history. Under these assumptions, the posterior for full SLAM factors as: p(x 1:t , m| z 1:t , u 1:t )â η p(x 1 ) p(m) t â i=1 p(z i | x i , m) tâ1 â i=1 p(x i+1 | x i , u i ),(14.4) where p(x 1 ) is the prior over the initial state, p(m) is the prior over the map, and η is a normalization constant. Each factor in this expression corresponds directly to one of the components of the SLAM model: âą p(x i+1 | x i , u i ) is a motion factor (odometry, IMU, . . . ). âą p(z i | x i , m) is a measurement factor (rangeâbearing, image reprojection, scan alignment, . . . ). âą p(x 1 ) and p(m) are prior factors that anchor the solution. This factorization is the state-space version of the graphical models discussed in Section 14.1. It underlies both filtering-based and smoothing-based methods: âą Filtering. Filtering-based methods marginalize out older states to maintain a belief over the current state and map: bel(x t , m) : = p(x t , m| z 1:t , u 1:t ), and update it as new controls and measurements arrive. Applying Bayesâ rule and the Markov assumptions yields the familiar Bayes-filter recursion: 362 principles of robot autonomy Prediction (motion update): bel(x t+1 , m) = Z p(x t+1 | x t , u t ) bel(x t , m) dx t , Correction (measurement update): bel(x t+1 , m) = η p(z t+1 | x t+1 , m) bel(x t+1 , m), where η normalizes the belief to integrate to 1. âą Smoothing.Smoothing-based methods operate directly on Equation (14.4), keeping the entire trajectory x 1:t and map m as variables. Under Gaussian assumptions, maximizing the posterior p(x 1:t , m | z 1:t , u 1:t ) is equivalent to solving a sparse nonlinear least-squares problem whose terms correspond to the factors in Equation (14.4)âthis is the optimization viewpoint developed in Sections 14.7 and 14.8. The rest of this chapter introduces what can be interpreted as different ways of approximating and exploiting this factorization: âą EKF-SLAM (Section 14.5) implements the Bayes-filter recursion directly under Gaussian noise and first-order linearization. âą FastSLAM and related particle-filter methods (Section 14.6) use sampling over trajectories combined with conditional Gaussian subproblems for map fea- tures. âą Graph- and factor-graph-based SLAM (Sections 14.7 and 14.8) encode the fac- tors in Equation (14.4) explicitly in a graphical model and solve for a MAP estimate via sparse nonlinear optimization. In each case, the underlying probabilistic structure is the same; what differs is how we represent the belief and which approximations we make to render com- putation tractable. 14.5 Extended Kalman Filter SLAM We have already encountered the EKF in two contexts: first as a general state estimation algorithm in Chapter 12, and then as a localization method when a map is known in Chapter 13. We now extend this idea to the SLAM setting. In EKF-SLAM, the map is treated as part of an augmented state vector, and the joint posterior over robot pose and map is updated recursively under Gaus- sian noise assumptions and first-order linearizations of the process and measure- ment models 22 . This generalizes the earlier uses of the EKF from estimating only 22 R. Smith, M. Self, and P. Cheeseman. âEstimating uncertain spatial relation- ships in roboticsâ. In: Autonomous robot vehicles. Springer, 1990, p. 167â193, J. J. Leonard and H. F. Durrant-Whyte. âSi- multaneous map building and localiza- tion for an autonomous mobile robot.â In: IROS. vol. 3. 1991, p. 1442â1447 the robotâs state to simultaneously estimating a static feature-based map and the evolving trajectory. As in Chapter 13, we assume that the map is feature-based: m =m 1 , m 2 , . . . , m N , simultaneous localization and mapping (slam) 363 where m i is the i-th feature with coordinates m i,x , m i,y in a 2D environment. The joint state vector at time t is then: y t : = " x t m # ,(14.5) and the online SLAM goal is to compute the posterior belief: bel(y t ) = p(x t , m| z 1:t , u 1:t ). We consider a state transition model for the augmented state y t of the form: y t = g(y tâ1 , u t ) + Δ t , with additive Gaussian process noise Δ t ⌠N (0, Q t ). The nonlinear function g is defined as: g(y tâ1 , u t ) = ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° f (x tâ1 , u t ) m 1,tâ1 . . . m N,tâ1 ïŁč ïŁș ïŁș ïŁș ïŁș ïŁ» , where f denotes the robot motion model, and we assume that each map fea- ture m i is static, so its process model is the identity. The process noise covariance has block structure: Q t = " Ì Q t 0 00 # , where Ì Q t is the motion model noise covariance for the robot, and the zero blocks reflect the assumption of no process noise for the map features. The Jacobian of the augmented motion model is: G t =â y g(y, u t ) y=ÎŒ tâ1 , that is, the derivative of g with respect to the augmented state, evaluated at the current mean estimate ÎŒ tâ1 . The measurement model mirrors the localization setting in Chapter 13: z i t = h(y t , j) + ÎŽ t , where ÎŽ t ⌠N (0, R t ) is zero-mean Gaussian noise and j is the index of the map feature m j â m associated with measurement i. The Jacobian of the measurement model is: H j t =â y h(y, j) y= ÌÎŒ t , the derivative of the measurement function with respect to the augmented state, evaluated at the predicted mean ÌÎŒ t obtained from the EKF prediction step. 364 principles of robot autonomy Algorithm 14.1: EKF Online SLAM with Known Correspondences Data: ÎŒ tâ1 ,ÎŁ tâ1 , u t , z t , c t Result: ÎŒ t ,ÎŁ t // Prediction step: propagate belief with motion model ÌÎŒ t = g(ÎŒ tâ1 , u t ) Ì ÎŁ t = G t ÎŁ tâ1 G T t + Q t // Correction step: process each measurement foreach z i t do j = c i t // index of associated map feature if feature j has never been seen before then Initialize " Ì ÎŒ j,x Ì ÎŒ j,y # as the expected position based on z i t // Innovation covariance S i t = H j t Ì ÎŁ t [H j t ] T + R t // Kalman gain K i t = Ì ÎŁ t [H j t ] T [S i t ] â1 // State update using measurement residual ÌÎŒ t = ÌÎŒ t + K i t (z i t â h( ÌÎŒ t , j)) // Covariance update Ì ÎŁ t = (Iâ K i t H j t ) Ì ÎŁ t // Final posterior belief ÎŒ t = ÌÎŒ t ÎŁ t = Ì ÎŁ t return ÎŒ t ,ÎŁ t 14.5.1 EKF-SLAM with Known Correspondences As in EKF localization, it is instructive to first consider the case in which the data associations are known. Let c t = [c 1 t , . . . ] †denote the correspondence vector, where c i t is the index of the map feature associated with measurement z i t . With known correspondences, the EKF-SLAM algorithm shown in Algorithm 14.1 is nearly identical to the EKF localization algorithm in Algorithm 13.2, except that it operates on the augmented state y containing both robot pose and map. A typical initialization for the belief bel(y 0 ) places the robot at the origin of the map frame with high confidence and assigns very weak priors to the features: ÎŒ 0 = ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° x 0 0 . . . 0 ïŁč ïŁș ïŁș ïŁș ïŁș ïŁ» ,ÎŁ 0 = ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° Ì ÎŁ 0 0 ·0 0â ·0 . . . . . . . . . . . . 00 ·â ïŁč ïŁș ïŁș ïŁș ïŁș ïŁ» , simultaneous localization and mapping (slam) 365 where: x 0 = ïŁź ïŁŻ ïŁŻ ïŁ° 0 . . . 0 ïŁč ïŁș ïŁș ïŁ» , Ì ÎŁ 0 = ïŁź ïŁŻ ïŁŻ ïŁ° 0 ·0 . . . . . . . . . 0 ·0 ïŁč ïŁș ïŁș ïŁ» , and x 0 and Ì ÎŁ 0 are the initial robot state and its covariance. The large feature co- variance termsâconceptually infiniteâexpress complete lack of prior knowledge about landmark locations. When a feature is first observed, the algorithm reini- tializes its mean using the corresponding measurement, rather than linearizing the measurement function about an arbitrary initial guess such as the origin. For a rangeâbearing sensor in 2D, the geometry is simple. Suppose the robot pose is (x t , y t , Ξ t ) in the global frame, and the sensor returns a range r and bearing Ï to a previously unseen feature in the robot frame. In the robot frame, the feature lies at: " r cos Ï r sin Ï # . To express this in the global frame, we first rotate by the robot heading Ξ t and then translate by the robot position: " m x m y # = " x t y t # + " cos Ξ t â sin Ξ t sin Ξ t cos Ξ t #" r cos Ï r sin Ï # . Using trigonometric identities, this simplifies to: m x = x t + r cos(Ξ t + Ï),m y = y t + r sin(Ξ t + Ï). This is the formula used in the algorithm to initialize a landmark from a single rangeâbearing observation. Because the sensor provides both range and bearing, a single measurement suffices to place the landmark (up to measurement noise). By contrast, for pure-bearing sensors such as a monocular camera without depth information, multiple views and robot motion are required to infer landmark positions, as discussed in the visual SLAM examples of Section 14.3. 14.5.2 EKF-SLAM with Unknown Correspondences So far, we have assumed that each measurement z i t is already matched to a map feature via a correspondence index c i t . In reality, these correspondences are rarely known and must be inferred online. This makes SLAM substantially harder than the known-map localization problem in Chapter 11 because the map itself is un- certain. Conceptually, the main new task is to decide, for every incoming measure- ment, whether it should be assigned to an existing feature (data association) or interpreted as a new feature (map expansion). Both choices affect subsequent es- timates: an incorrect association can corrupt the map, while an unnecessary new feature increases complexity. 366 principles of robot autonomy A common strategy is to choose correspondences by maximum likelihood. For each measurement z i t , we evaluate its likelihood under each possible landmark hypothesis and select the most plausible one. Given a predicted belief bel(y t ) after the motion update, the predictive distri- bution for a measurement z i t associated with landmark j is the distribution of z i t implied by the current uncertainty over y t and the measurement model. For- mally: p(z i t | z 1:tâ1 , u 1:t , c i t = j) = Z p(z i t | y t , c i t = j)bel(y t ) dy t . Under the EKF assumptions of a Gaussian belief and linearized measurement model, this integral can be computed in closed form and yields a likelihood Gaus- sian distributionN (z i t | Ëz j t , S j t ) with mean: Ëz j t = h( ÌÎŒ t , m j ), and covariance: S j t = H j t Ì ÎŁ t [H j t ] †+ R t , exactly as in the EKF localization case. Maximizing this likelihood with respect to j is equivalent (see Chapter 11) to minimizing the Mahalanobis distance: d ij t = (z i t â Ëz j t ) †[S j t ] â1 (z i t â Ëz j t ).(14.6) Intuitively, d ij t measures how many âstandard deviationsâ the actual measure- ment z i t lies from the predicted measurement Ëz j t , taking into account the full covariance. Compared to Euclidean distance, the Mahalanobis distance automat- ically downweights directions of high uncertainty and emphasizes directions of low uncertainty. Decision rule and Ï 2 gating. The unknown-correspondence EKF-SLAM loop adds one more decision layer on top of Algorithm 14.1: 1. For each measurement z i t , hypothesize a potential new feature position, for example by triangulating from rangeâbearing measurements, which would in- crease the feature count from N tâ1 to N t = N tâ1 + 1. 2. For all existing features k = 1, . . . , N t , compute the Mahalanobis distance d ik t between z i t and the prediction for feature k: Ëz k t = h( ÌÎŒ t , k),S k t = H k t Ì ÎŁ t [H k t ] T + R t , and: d ik t = (z i t â Ëz k t ) †[S k t ] â1 (z i t â Ëz k t ). 3. If all d ik t are âtoo largeâ, treat z i t as a new feature; otherwise, assign the mea- surement to the feature with the smallest Mahalanobis distance. simultaneous localization and mapping (slam) 367 To formalize the notion of âtoo largeâ, it is common to use a Ï 2 gate. For a d-dimensional measurement and a correctly specified Gaussian model, the Ma- halanobis distance d ij t follows a Ï 2 distribution with d degrees of freedom. This means that if a landmark hypothesis is correct, d ij t will lie below a chosen thresh- old most of the time. We therefore pick a threshold α such that: Pr Ï 2 d †α = p, where p is a desired confidence level, such as p = 0.95. The region d ij t †α is then a p-confidence âellipseâ in measurement space: we accept an association only if the measurement falls inside this ellipse. In practice: âą If d ik t †α for some k, we select the feature with the smallest d ik t as the best match. âą If d ik t > α for all existing features, we consider z i t to be a new landmark and initialize it accordingly. The complete EKF-SLAM algorithm for unknown correspondences is summa- rized in Algorithm 14.2. Although conceptually straightforward, EKF online SLAM with unknown cor- respondences is rarely robust enough for large, cluttered environments. Spurious measurements can create false landmarks that persist indefinitely, and errors in association may contaminate both the map and the pose estimates. Mitigation strategies include stronger outlier rejection in the front-end, more distinctive fea- ture descriptors, and conservative validation gates. A further limitation is that the computational and memory requirements of EKF-SLAM scale quadratically with the number of features N, making it challenging to scale to very large maps. Example 14.5.1 (Differential drive robot with range and bearing measurements.). Consider a differential drive robot with state consisting of two-dimensional posi- tion and heading, x = [x, y, Ξ] †. Suppose a sensor is available that measures the range, r, and bearing, Ï, to features m j â m relative to the robotâs local frame. At each time step, multiple measurements are collected: z t =[r 1 t , Ï 1 t ] †, [r 2 t , Ï 2 t ] †, . . ., where each measurement z i t = [r i t , Ï i t ] †. For SLAM, define the augmented state: y t : = ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° x t m 1 . . . m N ïŁč ïŁș ïŁș ïŁș ïŁș ïŁ» = h xy Ξm 1,x m 1,y . . .m N,x m N,y i †. With known correspondences, the measurement model for feature j is: h(y t , j) = " q (m j,x â x) 2 + (m j,y â y) 2 atan2(m j,y â y, m j,x â x)â Ξ # . 368 principles of robot autonomy Algorithm 14.2: EKF Online SLAM with Unknown Correspondences Data: ÎŒ tâ1 ,ÎŁ tâ1 , u t , z t , N tâ1 Result: ÎŒ t ,ÎŁ t N t = N tâ1 // Prediction ÌÎŒ t = g(ÎŒ tâ1 , u t ) Ì ÎŁ t = G t ÎŁ tâ1 G T t + Q t // Process each measurement foreach z i t do Estimate position " Ì ÎŒ N t +1,x Ì ÎŒ N t +1,y # from z i t foreach k = 1 to N t + 1 do Ëz k t = h( ÌÎŒ t , k) S k t = H k t Ì ÎŁ t [H k t ] T + R t d ik t = (z i t â Ëz k t ) †[S k t ] â1 (z i t â Ëz k t ) d i(N t +1) t = α j = arg min k d ik t N t = maxN t , j K i t = Ì ÎŁ t [H j t ] T [S j t ] â1 ÌÎŒ t = ÌÎŒ t + K i t (z i t â Ëz j t ) Ì ÎŁ t = (Iâ K i t H j t ) Ì ÎŁ t ÎŒ t = ÌÎŒ t ÎŁ t = Ì ÎŁ t return ÎŒ t ,ÎŁ t The associated Jacobian H j t , corresponding to a measurement from feature j, is: H j t = ïŁź ïŁ° â Ì ÎŒ j,x â Ì ÎŒ t,x â q t,j â Ì ÎŒ j,y â Ì ÎŒ t,y â q t,j 00. . .0 Ì ÎŒ j,x â Ì ÎŒ t,x â q t,j Ì ÎŒ j,y â Ì ÎŒ t,y â q t,j 0. . . Ì ÎŒ j,y â Ì ÎŒ t,y q t,j â Ì ÎŒ j,x â Ì ÎŒ t,x q t,j â1 0. . .0 â Ì ÎŒ j,y â Ì ÎŒ t,y q t,j Ì ÎŒ j,x â Ì ÎŒ t,x q t,j 0. . . ïŁč ïŁ» , where: q t,j = ( Ì ÎŒ j,x â Ì ÎŒ t,x ) 2 + ( Ì ÎŒ j,y â Ì ÎŒ t,y ) 2 , and Ì ÎŒ j,x and Ì ÎŒ j,y are the estimates of the x and y coordinates of feature m j ex- tracted from ÌÎŒ t . Given both range and bearing measurements, we can initialize the estimated position of feature m j using: " Ì ÎŒ j,x Ì ÎŒ j,y # = " Ì ÎŒ t,x Ì ÎŒ t,y # + " r i t cos(Ï i t + Ì ÎŒ t,Ξ ) r i t sin(Ï i t + Ì ÎŒ t,Ξ ) # , which can be used in the known-correspondence EKF-SLAM algorithm in Algo- rithm 14.1 to initialize feature positions. In the unknown-correspondence case simultaneous localization and mapping (slam) 369 of Algorithm 14.2, similar triangulation can be used to hypothesize new fea- tures. Interactive code for this example (with known correspondences) is avail- able in the repository github.com/StanfordASL/pora-exercises in the notebook ch14/range _ bearing _ ekf _ slam.ipynb. While EKF-SLAM provides a principled probabilistic framework for joint pose and map estimation, its quadratic scaling in the number of features and its de- pendence on Gaussian assumptions and linearization limit performance in large or highly nonlinear environments. These limitations have motivated more scal- able and flexible alternatives, particularly particle filter-based and graph-based approaches, which we now discuss. 14.6 Particle Filter-Based SLAM The SLAM problem can also be tackled with nonparametric particle filters. A major advantage of this family of methods is that it fits naturally with the full SLAM formulation: a particle can represent an entire trajectory x 1:t of the robot, not just its current pose. In other words, the state of the particle filter is the whole path history, and the filter approximates the path posterior: p(x 1:t | z 1:t , u 1:t , c 1:t ), which is exactly the object of interest in full SLAM (see Section 14.4). Once we have a set of sampled trajectories, we can condition on each trajectory and reason about the map. This stands in contrast to EKF-SLAM, which works directly in the online SLAM setting by maintaining a single Gaussian belief over the current pose and map. A naive approach would treat the entire augmented state y t from Equation (14.5) as the state of a particle filter, in analogy with MCL in Chapter 13. In practice, however, this is infeasible: the number of particles required to approximate the belief grows rapidly with the state dimension, and a realistic map may contain hundreds or thousands of features. The key insight behind particle-based SLAM (and FastSLAM in particular) is that, given the full robot path and known correspondences, the locations of individual map features become conditionally independent. Formally, the SLAM posterior over y 1:t = (x 1:t , m) can be factored as: p(y 1:t | z 1:t , u 1:t , c 1:t ) = p(x 1:t | z 1:t , u 1:t , c 1:t ) N â i=1 p(m i | x 1:t , z 1:t , c 1:t ),(14.7) whose derivation we present in more detail in Equation (14.8). This factorization separates the SLAM posterior into: âą a path posterior p(x 1:t | z 1:t , u 1:t , c 1:t ) over robot trajectories, and âą individual feature posteriors p(m i | x 1:t , z 1:t , c 1:t ) for each map element. 370 principles of robot autonomy The idea behind particle-based SLAM is to approximate the path posterior with a particle filter while maintaining each feature posterior with a parametric estima- tor conditioned on the sampled path 23 . This reduces the effective dimensionality 23 The feature posterior is usually Gaus- sian. of the state space represented by particles: the map variables are handled analyt- ically, and sampling is required only for the robot trajectory. As with other particle-based methods, particle SLAM can: (i) handle nonlin- ear process and measurement models without explicit linearization, (i) repre- sent multimodal distributions, and (i) avoid computing Jacobians. On the other hand, particle methods may require a large number of samples to avoid degener- acy in higher dimensions, and their performance depends heavily on the choice of proposal distributions and resampling strategies. Factoring the posterior. Let the full augmented state be y 1:t = ( x 1:t , m ) and as- sume a single measurement per time step with a known correspondence c 1:t . The factorization in Equation (14.7) can be written more explicitly as: p(y 1:t | z 1:t , u 1:t , c 1:t ) = p(x 1:t | z 1:t , u 1:t , c 1:t ) N â i=1 p(m i | x 1:t , z 1:t , c 1:t ),(14.8) where m i is the i-th feature in the map m, the term p(x 1:t | z 1:t , u 1:t , c 1:t ) is the path posterior, and the terms p(m i | x 1:t , z 1:t , c 1:t ) are the feature posteriors. We derive this factorization as follows. First, by Bayesâ rule: p(y 1:t | z 1:t , u 1:t , c 1:t ) = p(x 1:t | z 1:t , u 1:t , c 1:t ) p(m| x 1:t , z 1:t , u 1:t , c 1:t ). Conditioning the feature posterior on x 1:t renders the past controls redundant, so: p(y 1:t | z 1:t , u 1:t , c 1:t ) = p(x 1:t | z 1:t , u 1:t , c 1:t ) p(m| x 1:t , z 1:t , c 1:t ). Next, consider the feature posterior p(m | x 1:t , z 1:t , c 1:t ) and focus on a partic- ular feature m i . We distinguish two cases according to whether this feature is observed at time t: if i Ìž= c t , feature m i is not observed, whereas if i = c t , it is. Under these two cases we get: p(m i | x 1:t , z 1:t , c 1:t ) = ïŁ± ïŁČ ïŁł p(m i | x 1:tâ1 , z 1:tâ1 , c 1:tâ1 ),iÌž= c t , p(z t |m i ,x t ,c t ) p(m i |x 1:tâ1 ,z 1:tâ1 ,c 1:tâ1 ) p(z t |x 1:t ,z 1:tâ1 ,c 1:t ) ,i = c t , where the first case simply states that an unobserved feature cannot be updated by the latest measurement, and the second follows from Bayesâ rule together with conditional independence of features given the trajectory. For the observed feature (i = c t ), we may also write: p(m c t | x 1:tâ1 , z 1:tâ1 , c 1:tâ1 ) = p(z t | x 1:t , z 1:tâ1 , c 1:t ) p(m c t | x 1:t , z 1:t , c 1:t ) p(z t | m c t , x t , c t ) . We now show that the factorization in Equation (14.8) holds by induction. Assume that at time tâ 1 the feature posterior factors as 24 : 24 This is trivially true at the first time step because there is not yet any infor- mation coupling the features. simultaneous localization and mapping (slam) 371 p(m| x 1:tâ1 , z 1:tâ1 , c 1:tâ1 ) = N â i=1 p(m i | x 1:tâ1 , z 1:tâ1 , c 1:tâ1 ). Then: p(m| x 1:t , z 1:t , c 1:t ) = p(z t | m, x t , c t ) p(m| x 1:tâ1 , z 1:tâ1 , c 1:tâ1 ) p(z t | x 1:t , z 1:tâ1 , c 1:t ) , = p(z t | m c t , x t , c t ) p(z t | x 1:t , z 1:tâ1 , c 1:t ) N â i=1 p(m i | x 1:tâ1 , z 1:tâ1 , c 1:tâ1 ). Substituting the two cases of iÌž= c t and i = c t for p(m i | x 1:t , z 1:t , c 1:t ) yields: p(m| x 1:t , z 1:t , c 1:t ) = p(m c t | x 1:t , z 1:t , c 1:t ) â iÌž=c t p(m i | x 1:t , z 1:t , c 1:t ) = N â n=1 p(m n | x 1:t , z 1:t , c 1:t ), which proves the factorization by induction. The factorization in Equation (14.8) says that once we fix a particular trajectory x 1:t , each landmark can be estimated independently from the others. This is precisely what FastSLAM exploits: particles are used only to represent different hypotheses over the trajectory, while each particle carries an analytical estimate of every landmark conditioned on that trajectory. 14.6.1 FastSLAM with Known Correspondences The factorization in Equation (14.8) forms the basis of FastSLAM, a particle-based SLAM algorithm that exploits this structure for computational efficiency. Fast- SLAM uses a particle filter to represent the path posterior p(x 1:t | z 1:t , u 1:t , c 1:t ) and, for each particle, maintains a separate EKF for each map feature representing p(m i | x 1:t , z 1:t , c 1:t ). The complete procedure is summarized in Algorithm 14.3. In this scheme, the set of particles is: P t : =P [1] t , P [2] t , . . . , P [K] t , where the k-th particle is: P [k] t : =x [k] t , ÎŒ [k] 1,t ,ÎŁ [k] 1,t , . . . , ÎŒ [k] N,t ,ÎŁ [k] N,t , where x [k] t denotes a trajectory hypothesis for the robot state and (ÎŒ [k] i,t ,ÎŁ [k] i,t ) the EKF mean and covariance for feature m i under that trajectory. For each particle, we thus maintain one EKF per feature; with K particles and N features, there are NK independent EKFs in total. Each EKF operates in a low-dimensional state space 25 , so these updates remain inexpensive even when the full map is large. 25 Typically 2D or 3D for a landmark. Algorithmic structure. The FastSLAM recursion closely resembles a particle filter, augmented with EKF feature updates: 372 principles of robot autonomy 1. Prediction (motion update). For each particle, sample a new robot pose x [k] t from the state transition model given the control input u t : x [k] t ⌠p(x t | x [k] tâ1 , u t ). 2. Feature update (measurement correction). For the observed feature j = c t , update the EKF mean and covariance in each particle: Ëz [k] = h(ÎŒ [k] j,tâ1 , x [k] t ), S = H j ÎŁ [k] j,tâ1 [H j ] †+ Q t , K =ÎŁ [k] j,tâ1 [H j ] †S â1 , ÎŒ [k] j,t = ÎŒ [k] j,tâ1 + K(z t â Ëz [k] ),ÎŁ [k] j,t = (Iâ K H j )ÎŁ [k] j,tâ1 . 3. Weighting. Assign each particle a weight w [k] proportional to the measurement likelihood under its map estimate: w [k] â exp â 1 2 (z t â Ëz [k] ) †S â1 (z t â Ëz [k] ) . 4. Copying unchanged features. For all features nÌž= c t , keep the corresponding EKF parameters unchanged: ÎŒ [k] n,t = ÎŒ [k] n,tâ1 ,ÎŁ [k] n,t =ÎŁ [k] n,tâ1 . 5. Resampling. Draw a new particle set P t by resampling from the weighted par- ticles, favoring those that explain the measurements well. FastSLAM is thus a hybrid algorithm: it uses a particle filter to represent the distribution over trajectories and, within each particle, uses EKFs to maintain Gaussian estimates for each feature. This combination avoids the worst of the curse of dimensionality by sampling only over robot state, not over the entire map. Unknown correspondences. So far we have assumed that correspondences c t are known. In practice, this is rarely the case. FastSLAM can be extended to treat correspondences as latent variables as well, leading to algorithms often referred to as FastSLAM 2.0 26 and related variants 27 . 26 M. Montemerlo et al. âFastSLAM 2.0: An Improved Particle Filtering Algo- rithm for Simultaneous Localization and Mapping that Provably Convergesâ. In: Proceedings of the 18th National Confer- ence on Artificial Intelligence (AAAI). 2003, p. 1151â1156 27 S. Thrun, W. Burgard, and D. Fox. Probabilistic Robotics. MIT Press, 2005 At a high level, within each particle we can: âą Evaluate the likelihood of an observation under multiple existing features us- ing, for example, Mahalanobis-distance gating as in EKF-SLAM. âą Consider the hypothesis that the observation corresponds to a new feature and initialize a new EKF state for it. âą Update the particleâs weight by marginalizing over these correspondence hy- potheses, or by selecting the most likely association within that particle. simultaneous localization and mapping (slam) 373 Algorithm 14.3: FastSLAM Data: P tâ1 , u t , z t , c t Result: P t for k = 1 to K do // Prediction: Sample new robot pose Sample x [k] t ⌠p(x t | x [k] tâ1 , u t ) // Measurement update for observed feature j = c t if feature j never seen before then Initialize feature: (ÎŒ [k] j,tâ1 ,ÎŁ [k] j,tâ1 ) else Ëz [k] = h(ÎŒ [k] j,tâ1 , x [k] t ) S = H j ÎŁ [k] j,tâ1 [H j ] T + R t K =ÎŁ [k] j,tâ1 [H j ] T [S] â1 ÎŒ [k] j,t = ÎŒ [k] j,tâ1 + K(z t â Ëz [k] ) ÎŁ [k] j,t = (Iâ K H j )ÎŁ [k] j,tâ1 // Weighting: compute importance weight w [k] = det(2ÏS) â1/2 exp â 1 2 (z t â Ëz [k] ) †S â1 (z t â Ëz [k] ) // Carry over unchanged features for nâ1, . . . , N, nÌž= c t do ÎŒ [k] n,t = ÎŒ [k] n,tâ1 ÎŁ [k] n,t =ÎŁ [k] n,tâ1 // Resampling: Select new particle set according to weights P t =â for i = 1 to K do Draw k with probabilityâ w [k] t P t =P t âȘ (x [k] t , ÎŒ [k] 1,t ,ÎŁ [k] 1,t , . . . , ÎŒ [k] N,t ,ÎŁ [k] N,t ) returnP t This effectively embeds a data association procedure inside each particle: a par- ticle whose map explains the observations well under some correspondence as- signment receives a larger weight and is more likely to survive resampling. While this strategy increases robustness to data association errors, it also in- creases computational cost, since each particle maintains its own local map and solves its own correspondence problem. In practice, FastSLAM with unknown correspondences is often combined with strong front-end outlier rejection, careful gating, and heuristics to keep the number of candidate associations manageable. Detailed treatments can be found in Thrun, Burgard, and Fox [148, Ch. 13] and subsequent work on RaoâBlackwellized particle filters for SLAM. 374 principles of robot autonomy 14.7 Graph SLAM In the previous sections, we treated SLAM in full generality: the unknowns in- cluded both the robot trajectory and a (possibly large) map. In many applica- tions, however, we are primarily interested in a consistent trajectory expressed in a global frame, while the environment is represented implicitly through relative pose constraints between robot states. This situation occurs, for example, when dense maps are built from aligned scans outside the optimization loop, or when a separate mapping module consumes the estimated poses. These specific characteristics of the problem, an environment represented im- plicitly through relative pose measurements between robot states, motivate an important extension of the general SLAM formulation: pose-graph SLAM. Here, the unknowns are restricted to the sequence of robot poses x 1:t , while landmarks and other map elements are either marginalized out or not represented explicitly. The resulting model is naturally expressed as a graph: âą Nodes correspond to robot poses x i at discrete times (or keyframes). âą Edges correspond to relative pose measurements between pairs of poses, such as odometry constraints between consecutive poses or loop-closure constraints between nonconsecutive poses. A generic relative pose measurement between poses i and j can be written as: z ij = h ij (x i , x j ) + ÎŽ ij , where z ij is the measured relative transformation from pose i to pose j, h ij is the measurement function (often a composition of rigid transformations), and ÎŽ ij is zero-mean noise with known covariance. Under Gaussian noise assumptions, the MAP estimate of the poses is obtained by minimizing the sum of squared, information-weighted residuals: x â 1:t = arg min x 1:t â (i,j)âE r ij (x i , x j ) 2 ⊠ij , where E is the set of edges in the graph,⊠ij is the information matrix associated with measurement z ij , and: r ij (x i , x j ) : = z ij â h ij (x i , x j ), is the residual. The information matrix is the inverse of the measurement co- variance,⊠ij = R â1 ij . Directions with high measurement variance correspond to low information (small entries in⊠ij ), and conversely, directions with low vari- ance represent high information. The weighted norm â„râ„ 2 ⊠= r †âŠr therefore penalizes residuals more strongly in directions where the sensor is reliable. The operatorâ denotes the relative pose difference on SE(2) or SE(3), the spaces of 2D or 3D rigid-body transformations. Given two poses x a , x b â SE(3), the expression: x a â x b = log x â1 a x b , simultaneous localization and mapping (slam) 375 maps the transformation from x a to x b to a vector in a local linear space (a 3D vector for SE(2), or 6D for SE(3)). Here log(·) is the inverse of the exponential map used to represent small rotations and translations. This construction allows us to compute residuals as ordinary vectors, while still respecting the underlying geometry of rotations and translations. Linearization and Jacobians. Because the residuals are generally nonlinear in x i and x j , we solve the MAP problem iteratively 28 . At each iteration, the residuals 28 For example, using GaussâNewton or LevenbergâMarquardt methods. are linearized around the current pose estimates. Denote the Jacobians of r ij as: A i = âr ij âx i ,A j = âr ij âx j . These matrices describe how the residual for edge (i, j) changes under small per- turbations of the connected poses. Collecting these Jacobians over all edges gives the linearized relationship between pose increments and residuals. Normal equations and sparsity. The linearized least-squares problem leads to the (sparse) normal equations: Hâx = b, whereâx is the stacked vector of pose increments for all nodes. The global information (Hessian) matrix H is obtained by summing contributions from all edges: H = â (i,j)âE J †ij ⊠ij J ij , where J ij stacks the Jacobians A i and A j , and b is the corresponding gradient vector: b = â (i,j)âE J †ij ⊠ij r ij . Each measurement affects only a small number of poses (typically two), so most entries in H are zero: only the blocks corresponding to poses i and j are affected by measurement (i, j). This sparsity is what allows large pose graphs with thousands of poses and constraints to be optimized efficiently using sparse linear algebra techniques. Two practical refinements are crucial in real-world pose-graph SLAM. Robust kernels. Incorrect edgesâfor example, from wrong loop closures or cor- rupted sensor dataâcan strongly pull the solution away from the true trajectory if they are modeled with a simple quadratic loss. To mitigate this, we often re- place the quadratic termâ„r ij â„ 2 ⊠ij with a robust loss Ï(â„r ij â„ 2 ⊠ij ), such as a Huber or Tukey loss. These functions behave quadratically for small residuals (so inliers are treated as in ordinary least squares), but grow more slowly for large residuals, effectively downweighting measurements that are inconsistent with the majority of the data. Robust estimation of this kind is standard in bundle adjustment and pose-graph SLAM; see, for example, the discussions in Hartley and Zisserman [48] and Thrun, Burgard, and Fox [148]. 376 principles of robot autonomy Priors and gauge freedom. A pose graph contains only relative constraints between poses. Without additional information, the entire trajectory can be rotated or translated without changing the relative pose errors, so the optimization problem is underdetermined. To fix this gauge freedom, we add a prior 29 on one pose, 29 Also sometimes called an anchor. typically the first one, such as: x 1 âŒN (x prior 1 ,ÎŁ prior ), with x prior 1 set to the origin andÎŁ prior a small covariance. This prior pins the co- ordinate frame and renders the solution unique up to small variations consistent with the prior. Updating poses on the manifold. The solution of the linear system yields incre- mentsâx i that live in the local linear space attached to each pose (the tangent space of SE(2) or SE(3)). Robot poses themselves, however, must remain valid rigid-body transformations and cannot be updated by simple vector addition. To perform a valid update, we use an operation often called a retraction, which maps a tangent increment back onto the manifold of rigid transformations: x i â x i ââx i . Concretely, for SE(2) or SE(3), this is typically implemented via the exponential map: x i ââx i = x i · Exp(âx i ), where Exp(·) maps a small 3D (or 6D) vectorâx i to a corresponding rigid-body transformation (rotation plus translation), and the product x i · Exp(âx i ) composes the current pose with this small increment. In practice, the user of a SLAM library does not need to work with the Lie-group details explicitly. It is enough to understand that: âą Optimization computes small incremental motions as vectors. âą These increments are âappliedâ to the current poses using group composition rather than plain addition. Example 14.7.1 (Pose-graph SLAM vs. dead reckoning.). In the repository gith ub.com/StanfordASL/pora-exercises, the notebook ch14/pose _ graph _ slam.ipynb implements pose-graph SLAM for a differential- drive robot. The baseline âdead-reckoningâ trajectory is obtained by integrating odometry alone, without loop-closure constraints. As a result of dead reckoning, small errors accumulate over time and the path drifts. The pose-graph solu- tion augments odometry edges with loop-closure edges between nonconsecutive poses whenever the robot revisits a known place. Optimizing the graph adjusts the entire trajectory so that both odometry and loop-closure constraints are satis- fied as well as possible, dramatically reducing drift compared to dead reckoning. simultaneous localization and mapping (slam) 377 Algorithm 14.4: Pose-Graph (GraphSLAM) â Batch GaussâNewton / LevenbergâMarquardt Data: Initial poses x (0) 1:t (e.g., from odometry), edge setE with measurementsz ij and information matrices⊠ij , prior on root pose x prior 1 ,⊠prior , max iterations K, damping λ℠0 (LM), robust kernel Ï (optional) Result: Optimized poses x â 1:t xâ x (0) 1:t for k = 1 to K do Initialize normal equations: H â 0, bâ 0 foreach (i, j)âE do Ëz ij â h ij (x i , x j ) r ij â z ij â Ëz ij // pose residual on SE(2/3) A i , A j ââ x i ,x j r ij w ij â robust weight from Ï(â„r ij ℠⊠ij ) // set w ij = 1 if no Ï Ì âŠ ij â w ij ⊠ij // Scatter-add into sparse H, b H i += A †i Ì âŠ ij A i ,H ij += A †i Ì âŠ ij A j ,H j += A †j Ì âŠ ij A j b i += A †i Ì âŠ ij r ij , b j += A †j Ì âŠ ij r ij // Anchor to fix gauge H 11 +=⊠prior , b 1 +=⊠prior x prior 1 â x 1 // Solve for increment Solve (H + λI)âx = b with sparse Cholesky/QR // Retract on the manifold for i = 1 to t do x i â x i ââx i // retraction via Exp(·) on SE(2/3) ifâ„âxâ„ â < Δ or relative cost decrease < Ï then break return x Pose-graph SLAM casts consistent trajectory estimation as a sparse, nonlinear least-squares optimization problem defined over a graph of poses. Odometry and other local motion estimates appear as edges between consecutive nodes, while loop closures appear as edges between nonconsecutive nodes corresponding to revisited places. These loop-closure edges are particularly powerful: they intro- duce long-range constraints that âtie togetherâ distant parts of the trajectory and allow accumulated drift to be redistributed along the path. Because each measurement involves only a small subset of poses, the resulting Hessian matrix is sparse and can be solved efficiently using modern sparse linear algebra and incremental solvers. Pose-graph SLAM is widely used when relative pose constraints are the primary information source, and the same formulation 378 principles of robot autonomy extends naturally to multi-robot scenarios (with inter-robot edges) and to hybrid representations in which selected landmarks, sensor extrinsics, or biases are kept as additional variables. In the next section, we generalize this idea to factor graphs, which provide a more flexible and modular representation for SLAM and related estimation problems, and make it convenient to incorporate heterogeneous measurements and additional unknowns within a single unified framework. 14.8 Factor Graph SLAM In Section 14.7, we focused on pose-graph SLAM, in which the only unknown variables are robot poses and every measurement is expressed as a relative pose constraint between two poses. While pose-graph SLAM covers many important applications, real SLAM systems often contain additional unknowns: explicit landmark positions, sensor calibration parameters, biases, or even semantic in- formation. We need a representation that can accommodate all of these in a principled way. Factor-graph SLAM provides this generalization. Instead of having a graph whose nodes are only robot poses and whose edges are only relative-pose con- straints, we consider a graph in which: âą Nodes represent any unknown variable we want to estimate. âą Edges (referred to as factors) represent the probabilistic relation induced by a single measurement or prior on the subset of variables it involves. Pose-graph SLAM is elegant but restrictive: it assumes that every piece of information can ultimately be written as a relative pose between two robot states. In practice, this abstraction hides important modeling elements: âą Landmarks. If we want to maintain and refine explicit landmark locations, such as for long-term mapping or semantic reasoning, only representing poses is insufficient. âą Sensor parameters and biases. Camera intrinsics, lidarâIMU extrinsics, time off- sets, and slowly varying sensor biases often need to be estimated jointly with the trajectory. âą Multi-way constraints. Some measurements depend on more than two variables at once, such as a stereo observation that depends on a pose, a landmark, and stereo calibration. These cannot be expressed as simple pairwise poseâpose constraints. Factor graphs overcome these limitations by embedding SLAM into the broader framework of probabilistic graphical models. We already met this idea at a high level in Section 14.1, and here we turn it into a concrete optimization problem. As we will see, pose-graph SLAM appears as the special case where poses are the simultaneous localization and mapping (slam) 379 only variables and all factors connect at most two poses at a time. In this case, the factor-graph formulation reduces exactly to the pose-graph formulation and algorithm in Algorithm 14.4. Variables, factors, and the joint posterior. In a factor graph, we collect all unknowns into three (possibly overlapping) groups: X =x 1 , . . . , x t robot poses, L =â 1 , . . . , â M landmarks, Î =Ξ 1 , . . .sensor parameters, biases, . . . We denote the set of all variables by: Y : =X, L,Î. Each measurement or prior gives rise to a factor that ties together only the subset of variables it depends on. Let Y k â Y be the variables affected by the k-th measurement, and let Ï k (Y k ) denote the corresponding factor. Under standard conditional-independence assumptions, the joint posterior has the product form: p(Y | Z)â â k Ï k (Y k ),(14.9) where Z = z k denotes all measurements. Each factor Ï k can be interpreted as a (possibly unnormalized) likelihood term for measurement k given the variables Y k . Example 14.8.1 (A ternary factor.). Suppose a stereo camera at pose x t observes a point landmark â j . The stereo measurement z t,j depends on: âą The robot pose x t (through the camera pose). âą The landmark coordinates â j . âą Stereo calibration parameters Ξ, such as the baseline, camera intrinsics, etc. In factor-graph language, this is a ternary factor: Ï(x t , â j , Ξ)â p(z t,j | x t , â j , Ξ), connecting three variables simultaneously. Such higher-order constraints can- not be represented in a pure pose graph, which allows only pairwise poseâpose edges, but they appear naturally in a factor graph by simply allowing factors to affect more than two nodes. From probabilities to least squares. As in the previous sections, we assume that each measurement z k is modeled by a measurement function h k (Y k ) with additive Gaussian noise: z k = h k (Y k ) + ÎŽ k ,ÎŽ k âŒN (0, R k ). 380 principles of robot autonomy The associated factor is then: Ï k (Y k )â exp â 1 2 â„r k (Y k )â„ 2 R â1 k , where: r k (Y k ) : = z k â h k (Y k ), is the residual 30 andâ„râ„ 2 R â1 = r †R â1 r is the information-weighted squared norm. 30 Possibly defined in a tangent space, as in the pose-graph case. Substituting this into Equation (14.9) and taking the negative log-likelihood shows that computing a MAP estimate: Y â = arg max Y p(Y | Z), is equivalent to solving the nonlinear least-squares problem: Y â = arg min Y â k â„r k (Y k )â„ 2 ⊠k ,⊠k : = R â1 k .(14.10) This has exactly the same structure as the pose-graph objective, but now the variables Y include poses, landmarks, calibration parameters, and so on, and factors can involve any subset of them. Linearization, sparsity, and the normal equations. To solve 14.10, we use iterative methods such as GaussâNewton or LevenbergâMarquardt (LM), as in Section 14.7. At each iteration, we linearize every residual around the current estimate, Y (k) : r k (Y k ) â r k (Y (k) k ) + J k âY k , where J k is the Jacobian of r k with respect to the stacked variables Y k , andâY k is the stacked increment for those variables. Collecting the contributions from all factors leads to the linearized normal equations: HâY = b,(14.11) with: H = â k J †k ⊠k J k , b = â k J †k ⊠k r k . In other words, each factor contributes a local term J †k ⊠k J k to the global Hessian (information matrix) H and a local term J †k ⊠k r k to the gradient b. Crucially, each factor Ï k depends only on the variables in Y k . This means that: âą The Jacobian J k has nonzero columns only for those variables. âą The contribution J †k ⊠k J k affects only the corresponding blocks of H. As a result, most entries of H are zero: the matrix is sparse. This sparsity is the key to scalability, since sparse direct solvers with carefully chosen variable orderings can solve very large systems in time that grows almost linearly with the number of variables, rather than cubic in the dimension as in the dense case. simultaneous localization and mapping (slam) 381 Robust kernels and priors. As in pose-graph SLAM, we often replace the simple quadratic term â„r k â„ 2 ⊠k with a robust loss Ï(â„r k â„ 2 ⊠k ) to reduce the influence of outliers 31 . Priors are represented as additional factors, such as a prior on the first 31 For example, due to incorrect corre- spondences or spurious loop closures. pose or on calibration parameters, and they play the same role of fixing gauge freedoms and encoding prior knowledge. Landmark marginalization and the Schur complement. When there are many land- marks, it is often advantageous to eliminate them analytically from the linear system and solve directly for the remaining variables (typically poses and cali- bration parameters). This is achieved via the Schur complement, which we now introduce at a high level. Consider the linear system in Equation (14.11) with unknowns separated into two groups: Y = (X, L), where X are pose-like variables and L are landmarks. After reordering, the normal equations can be written in block form as: " H X H X L H LX H L #" âX âL # = " b X b L # . The idea of the Schur complement is to first expressâL in terms ofâX using the second block row: H LX âX + H L âL = b L , which, assuming H L is invertible, gives: âL = H â1 L (b L â H LX âX), and then substitute this into the first block row. The result is a reduced system in the pose variables alone: H X â H X L H â1 L H LX âX = b X â H X L H â1 L b L .(14.12) The matrix H X â H X L H â1 L H LX is called the Schur complement of H L in the full system. Once we solve Equation (14.12) forâX, we can recoverâL from the expression above if needed. In SLAM problems, H L is typically block-diagonal or very sparse because landmarks are conditionally independent given the poses and each landmark is observed by a small subset of poses. This makes H â1 L cheap to compute or apply. The Schur complement therefore allows us to: âą Reduce the dimension of the main linear system to solve to the number of pose and calibration variables. âą Still retain the information carried by landmark observations. This strategy is standard in bundle adjustment and large-scale visual SLAM. 382 principles of robot autonomy Manifold retraction and variable updates. After solving the linear system or its Schur-reduced version, we obtain an incrementâY for all variables. As in Sec- tion 14.7, these increments live in the tangent spaces of the corresponding man- ifolds 32 and cannot, in general, be added to the variables with ordinary vector 32 For example, SE(3) for poses and R 3 for Euclidean points. addition. To update the estimate, each variable yâ Y is updated via a retraction: yâ yâây,(14.13) where â maps a small vectorây in the tangent space at y back to a valid point on the manifold. Concretely: âą For pose variables on SE(2) or SE(3),â is implemented using the exponential map: xââx = x· Exp(âx), where Exp(âx) converts the small 3D/6D vectorâx into a rigid-body trans- form, and the dot denotes composition. âą For Euclidean variables, such as landmark positions or scalar biases,â reduces to simple addition: ââââ = â +ââ. Conceptually, the optimization alternates between: 1. Solving a linearized problem in a local coordinate system or tangent space. 2. Mapping the resulting update back to the nonlinear manifold via Equation (14.13). This is exactly the same pattern we saw for pose-graph SLAM, now generalized to arbitrary variable types. Batch and incremental factor-graph SLAM. The batch factor-graph SLAM algo- rithm is summarized in Algorithm 14.5. Algorithmically, it closely resembles the pose-graph optimizer in Algorithm 14.4 but operates on a larger set of variables and factors. GaussâNewton and LevenbergâMarquardt differ mainly in how the damp- ing parameter λ is chosen and updated. GaussâNewton corresponds to λ = 0 and works well when the initial estimate is close to the optimum. Levenbergâ Marquardt introduces a positive λ to make the linear system better conditioned and to interpolate between GaussâNewton and gradient descent when far from the solution. For large-scale and online problems, recomputing and refactorizing H from scratch at every iteration is wasteful. Incremental smoothing and mapping algo- rithms, such as iSAM and iSAM2, exploit the factor-graph structure to update only the affected parts of the solution when new measurements arrive, reusing previous computations and maintaining sparsity. 33 This enables real-time perfor- 33 M. Kaess, A. Ranganathan, and F. Del- laert.âiSAM: Incremental smoothing and mappingâ.In: IEEE Transactions on Robotics 24.6 (2008), p. 1365â1378, M. Kaess et al. âiSAM2: Incremental smoothing and mapping using the Bayes treeâ.In: The International Journal of Robotics Research 31.2 (2012), p. 216â235 mance in many practical SLAM systems. Factor-graph SLAM provides a unifying, modular view of SLAM back-ends: simultaneous localization and mapping (slam) 383 Algorithm 14.5: Factor-Graph SLAM: Batch GaussâNewton / Levenbergâ Marquardt Data: Variables Y =X, L,Î, . . . with initial guess Y (0) , factors F =Ï k , each with measurement z k , information⊠k , and model z k â h k (Y k ), optional ordering Ï, max number of iterations K, damping λ℠0, robust kernel Ï (optional) Result: MAP estimate Y â Y â Y (0) for t = 1 to K do H â 0, bâ 0 foreach Ï k âF do Ëz k â h k (Y k ) r k â z k â Ëz k // on tangent space J k ââ Y k r k w k â robust weight from Ï(â„r k ℠⊠k ) Ì âŠ k â w k ⊠k H += J †k Ì âŠ k J k , b += J †k Ì âŠ k r k if use Schur complement then Partition H, b into pose vs. landmark blocks and eliminate L Solve (H + λI)âY = b with sparse Cholesky/QR foreach variable yâ Y do yâ yâây ifâ„âYâ„ â < Δ or relative cost decrease < Ï then break return Y âą It generalizes pose-graph SLAM by allowing arbitrary variables (poses, land- marks, calibration, biases, . . . ) and factors of any arity. âą The joint posterior factors into local terms, leading to sparse Jacobians and Hessians and enabling scalable optimization. âą Landmark marginalization via the Schur complement and incremental solvers such as iSAM build directly on this structure to handle large-scale, real-time applications. Because of these advantages, factor graphs underpin nearly all modern SLAM back-ends and form the conceptual bridge between probabilistic modeling and the efficient numerical algorithms used in practice. 14.9 Advanced and Emerging Methods Classical SLAM pipelines rely primarily on geometric features and optimization- based back-ends. In recent years, however, the field has expanded to incorporate 384 principles of robot autonomy learning-based techniques, richer scene representations, and deeper integration with broader AI systems. These emerging methods aim not only to improve accuracy and robustness, but also to enable SLAM to operate in environments and applications beyond the reach of purely geometric methods. Deep learning in SLAM. Learning has been used to enhance both the front-end and back-end of SLAM. On the front-end, convolutional and transformer-based networks provide robust feature detection, semantic segmentation, and depth estimation even in challenging lighting or texture-poor settings. On the back- end, learned priors can regularize optimization, improve loop-closure detection, or better model uncertainty in sensor data. Beyond such modular uses, end-to-end âneural SLAMâ architectures attempt to replace hand-engineered pipelines entirely with learned models that ingest raw sensory streams and output pose and map estimates 34 . While promising, these 34 D. S. Chaplot et al. âLearning to ex- plore using active neural slamâ.In: arXiv preprint arXiv:2004.05155 (2020) approaches raise questions about generalization, interpretability, and robustness that remain active research topics. Neural implicit maps. Traditional mapping approachesâoccupancy grids, point clouds, and mesh reconstructionsâeither scale poorly or lack continuity and compactness. Neural implicit representations, such as neural signed distance functions and neural radiance fields, provide continuous, compact encodings of geometry and appearance. These maps can be queried at arbitrary resolution, fused across time, and potentially shared among multiple agents. Although computationally demanding, implicit maps suggest a new paradigm in which SLAM outputs not only metric geometry but also a photorealistic and semantically enriched âdigital twinâ of the environment. 14.9.1 Semantic and Dynamic SLAM Classic SLAM often assumes static scenes, but real-world environments are dy- namic and populated by moving agents. Semantic SLAM augments maps with object-level labels, enabling robots to recognize and reason about doors, vehicles, furniture, and other meaningful entities rather than anonymous landmarks. Dy- namic SLAM explicitly models moving objects, separating them from the static background and, in some cases, tracking them jointly with the ego-motion. These capabilities unlock task-driven autonomy, where maps support higher- level reasoning, interaction, and prediction, not just localization. 14.9.2 Toward Spatial AI Looking forward, SLAM is evolving beyond trajectory estimation and mapping toward a broader concept sometimes referred to as Spatial AI. Here, geometry, se- mantics, and temporal dynamics are tightly integrated into a coherent representa- tion that supports decision-making, planning, and interaction. Rather than being simultaneous localization and mapping (slam) 385 a self-contained module, SLAM becomes part of a larger perceptionâandâaction loop, enabling robots to act intelligently in complex, dynamic worlds. 14.10 Summary In this chapter, we developed a unified view of the SLAM problem as a corner- stone of robot perception and autonomy. We began by framing SLAM as a joint state estimation problem in which a robot must concurrently infer its own tra- jectory and a map of the environment from noisy sensor data. We introduced the Bayesian formulation that underlies both classical and modern approaches, emphasizing the Markov assumptions and probabilistic factorizations that make estimation tractable. We then explored the algorithmic paradigms that have shaped SLAM over the past three decades. Filter-based methods, such as the EKF SLAM and particle- filter approaches like FastSLAM, introduced recursive estimation frameworks capable of operating online. Smoothing-based methods, including graph- and factor-graph SLAM, reframed the SLAM problem as a sparse nonlinear least- squares optimization problem, enabling accurate and scalable solutions through modern sparse solvers and robust cost functions. The chapter further distinguished between the front-end, which extracts con- straints from raw sensor data (through feature detection, data association, and loop closure), and the back-end, which solves the underlying estimation problem. We discussed representative pipelines for different sensing modalitiesâincluding visual, lidar, and radar SLAMâand highlighted how sensor characteristics shape both front-end and back-end design. Finally, we surveyed recent advances that extend SLAM beyond purely ge- ometric mapping, encompassing learning-based front-ends, neural implicit rep- resentations, semantic and dynamic mapping, and the emerging paradigm of Spatial AI, which integrates geometry, semantics, and temporal reasoning into unified spatial representations. To learn more. Comprehensive treatments of probabilistic robotics and SLAM algorithms can be found in Thrun, Burgard, and Fox [148] and Leonard and Durrant-Whyte [79], which remain foundational references for understanding classical formulations. For a modern perspective emphasizing optimization and factor graphs, Carlone et al. [24] provides an accessible and rigorous overview of contemporary SLAM back-ends and the underlying estimation theory. Readers interested in practical implementations and ongoing research frontiersâparticularly in visualâinertial, semantic, and learning-based SLAMâare encouraged to con- sult recent surveys and open-source frameworks such as GTSAM, g2o, and ORB- SLAM. 386 principles of robot autonomy 14.11 Exercises The starter code for the exercises provided below is available online through GitHub. To get started, download the code by running in a terminal window: git clone https://github.com/StanfordASL/pora-exercises.git We denote Problems requiring hand-written solutions and coding in Python with and, respectively. Problem 1: EKF SLAM In this problem, you will implement an extended Kalman filter (EKF) SLAM algo- rithm for robot and landmark localization in an environment where the robot can collect relative position measurements to a set of four landmarks 35 . Specifically, 35 Note that this is the same problem setup as Problem 1 and 2 in Chapter 13. we consider a robot with a discrete-time dynamics model x t+1 = f (x t , u t ) + Δ t with the state being the robot pose, x t = [x t , y t , Ξ t ] †, and the dynamics are de- fined by: x t+1 = x t + V t cos(Ξ t )ât + Δ x t , y t+1 = y t + V t sin(Ξ t )ât + Δ y t , Ξ t+1 = Ξ t + Ï t ât + Δ Ξ t . The noise vector Δ t = [Δ x t , Δ y t , Δ Ξ t ] †is a random variable with a zero mean Gaussian distribution Δ t âŒN (0, Q), where Q = 0.1ât 2 I. In this problem, we assume there are four stationary landmarks in the environ- ment whose positions are unknown. We define the state vector for the landmark positions as: m = h m 1,x m 1,y m 2,x m 2,y m 3,x m 3,y m 4,x m 4,y i †. As the robot navigates through its environment, it receives noisy measurements of the positions of four landmarks in the environment relative to the robotâs cur- rent pose. The measurement for landmark i is the relative position with the measurement model: z i t = h(x t , i, m) + ÎŽ t = " cos(Ξ t )sin(Ξ t ) â sin(Ξ t )cos(Ξ t ) # " m i,x m i,y # â " x t y t # + ÎŽ t , where the measurements have associated noise ÎŽ t ⌠N (0, R t ), with R = 0.25I. The full measurement vector of all landmarks is: z t = ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁ° z 1 t z 2 t z 3 t z 4 t ïŁč ïŁș ïŁș ïŁș ïŁ» . simultaneous localization and mapping (slam) 387 In this exercise, we consider the SLAM problem of estimating simultaneously the robot state x and the landmark state m. We denote the combined state as y = [x, m] †and denote the combined dynamics model for this state as y t+1 = g(y t , u t ) + Δ t In the file ch14/exercises/ekf _ slam.ipynb, complete the following: 1. Implement the functions robot _ dynamics, robot _ measurement, and state _ dynamics that define the robotâs dynamics model and measurement model described above, as well as the dynamics model for the SLAM state y. 2. Implement the function dynamics _ jacobian to compute the dynamics Jacobian for the combined state G t =â y g(y t , u t ). 3. Implement the function measurement _ jacobian to compute the measurement model Jacobian H t = â y h(x t , m) for the model that computes the full mea- surement vector z t . Note we compute the measurement Jacobian with respect to the combined state y. 4. Implement the function ekf _ slam _ update to implement the EKF SLAM up- date. 5. Run the provided code to see how the algorithm performs for the simulated robot. 388 principles of robot autonomy References [24]L. Carlone et al., eds. SLAM Handbook. From Localization and Mapping to Spatial Intelligence. Cambridge University Press, 2026. [25]D. S. Chaplot et al. âLearning to explore using active neural slamâ. In: arXiv preprint arXiv:2004.05155 (2020). [27]F. Dellaert. âFactor graphs and GTSAM: A hands-on introductionâ. In: Georgia Institute of Technology, Tech. Rep 2.4 (2012). [28]F. Dellaert. âFactor graphs: Exploiting structure in roboticsâ. In: Annual Review of Control, Robotics, and Autonomous Systems 4.1 (2021), p. 141â166. [29]F. Dellaert and M. Kaess. âSquare root SAM: Simultaneous localization and mapping via square root information smoothingâ. In: The International Journal of Robotics Research 25.12 (2006), p. 1181â1203. [48]R. Hartley and A. Zisserman. âCamera Modelsâ. In: Multiple View Geome- try in Computer Vision. Academic Press, 2002. [62]M. Kaess, A. Ranganathan, and F. Dellaert. âiSAM: Incremental smooth- ing and mappingâ. In: IEEE Transactions on Robotics 24.6 (2008), p. 1365â 1378. [63]M. Kaess et al. âiSAM2: Incremental smoothing and mapping using the Bayes treeâ. In: The International Journal of Robotics Research 31.2 (2012), p. 216â235. [71]G. Klein and D. Murray. âParallel tracking and mapping for small AR workspacesâ. In: 2007 6th IEEE and ACM international symposium on mixed and augmented reality. IEEE. 2007, p. 225â234. [73]R. KĂŒmmerle et al. âg 2 o: A general framework for graph optimizationâ. In: 2011 IEEE international conference on robotics and automation. IEEE. 2011, p. 3607â3613. [79]J. J. Leonard and H. F. Durrant-Whyte. âSimultaneous map building and localization for an autonomous mobile robot.â In: IROS. Vol. 3. 1991, p. 1442â1447. [89]F. Lu and E. Milios. âRobot pose estimation in unknown environments by matching 2d range scansâ. In: Journal of Intelligent and Robotic systems 18.3 (1997), p. 249â275. [96]M. Montemerlo et al. âFastSLAM 2.0: An Improved Particle Filtering Al- gorithm for Simultaneous Localization and Mapping that Provably Con- vergesâ. In: Proceedings of the 18th National Conference on Artificial Intelli- gence (AAAI). 2003, p. 1151â1156. [97]M. Montemerlo et al. âFastSLAM: A factored solution to the simultaneous localization and mapping problemâ. In: Aaai/iaai 593598.2 (2002), p. 593â 598. simultaneous localization and mapping (slam) 389 [99]R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos. âORB-SLAM: A versatile and accurate monocular SLAM systemâ. In: IEEE transactions on robotics 31.5 (2015), p. 1147â1163. [113]T. Qin, P. Li, and S. Shen. âVins-mono: A robust and versatile monocular visual-inertial state estimatorâ. In: IEEE transactions on robotics 34.4 (2018), p. 1004â1020. [140]R. Smith, M. Self, and P. Cheeseman. âEstimating uncertain spatial rela- tionships in roboticsâ. In: Autonomous robot vehicles. Springer, 1990, p. 167â 193. [148]S. Thrun, W. Burgard, and D. Fox. Probabilistic Robotics. MIT Press, 2005. [163]J. Zhang, S. Singh, et al. âLOAM: Lidar odometry and mapping in real- time.â In: Robotics: Science and systems. Vol. 2. 9. Berkeley, CA. 2014, p. 1â 9. 15 Sensor Fusion and Object Tracking Individual sensors come with hard-wired limitations in terms of range, field of view, and resolution or quantization. On top of these design limits, performance often degrades under certain environmental conditions 1 . In practice, sensors also 1 M. Liggins I, D. Hall, and J. Llinas. Handbook of multisensor data fusion: theory and practice. CRC press, 2017, S. Thrun, W. Burgard, and D. Fox.Probabilistic Robotics. MIT Press, 2005 age and occasionally fail: calibration drifts, biases grow, and sometimes a device drops out altogether. The aim of sensor fusion is to design robotic systems that remain robust to individual sensor weaknesses by combining multiple, often het- erogeneous, sensors so that their information collectively reduces uncertainty in perception and localization tasks 2 . 2 F. Gustafsson. Statistical Sensor Fusion. Studentlitteratur, 2013, p. 554, D. Simon. Optimal State Estimation: Kalman, H â , and Nonlinear Approaches. John Wiley & Sons, 2006 Why fusion helps. Three recurring patterns motivate sensor fusion in real sys- tems: 1. Complementarity: Different sensor modalities observe different aspects of the world, such as geometry or appearance, and combining them reduces ambi- guity. 2. Redundancy: Multiple sensors that observe the same latent quantity allow re- silience to noise spikes and dropouts via consistency checks and cross-validation. 3. Cooperation: Two weak cues can form a strong one when combined, such as monocular vision paired with an IMU to recover metric scale. At a high level, sensor fusion treats each sensor output as a probabilistic ob- servation of the underlying state and then combines those observations to obtain a posterior belief that is sharper and more reliable than any single source. Example 15.0.1 (Perception for autonomous driving). A self-driving car typically uses a combination of lidar and radar for distance sensing. Lidar provides high- resolution geometric structure at short to medium range, while radar is more robust at longer ranges and in adverse weather 3,4 . Cameras complement dis- 3 S. Blackman and R. Popoli.Design and Analysis of Modern Tracking Systems. Artech House, 1999 4 Radar is also generally more robust than lidar in fog, rain, or snow. tance sensors: they provide high spatial density and rich appearance cues for object recognition and semantics, and they supply accurate bearing information to landmarks and obstacles. This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 392 principles of robot autonomy Chapter 15 illustrates the idea in 2D. Radar measurements often have strong longitudinal (range) accuracy but weaker lateral resolution; cameras, by contrast, deliver accurate lateral bearing but poor absolute depth. Fusing the two yields a positional estimate that is precise both longitudinally and laterally. Figure 15.1: Sensor fusion can reduce uncertainty by providing more well-rounded data. For ex- ample, a radar sensor may pro- vide good longitudinal distance accuracy but slightly less lateral accuracy, and a camera may pro- vide poor range estimation but good lateral position estimation. By fusing these two signals, the resulting position estimate can be accurate longitudinally and laterally. Example 15.0.2 (Localization for ground robots). A wheeled robot might use GNSS together with wheel encoders to estimate position. GNSS keeps global po- sition error bounded, while encoders provide fine-grained, high-rate short-term motion increments. When GNSS is degraded or lost, such as in urban canyons or under foliage, encoder odometry can bridge the gap. Conversely, when wheel slip corrupts odometry, GNSS can pull drift back into line. As we will see, treat- ing these sources probabilistically allows a filter to down-weight whichever one is less reliable at any moment. Example 15.0.3 (Other common pairings). Aerial robots often fuse a high-rate IMU with barometer and vision. The IMU stabilizes attitude and short-term motion, vision constrains drift and provides scale, and the barometer anchors altitude. Mobile manipulators combine joint encoders with depth sensing for precise end-effector placement while maintaining situational awareness. In all of these cases, no single sensor is sufficient across all operating regimes, but a carefully engineered combination is. We will adopt a probabilistic perspective on fusion. Let x denote the latent state and let Z = z (1) , z (2) , . . . be the set of measurements from all sensors. Under suitable assumptions, fusion is described by the posterior belief: p(x|Z)â p(x) â i p(z (i) | x), which we will realize with Kalman-family filters and their nonlinear and dis- tributed variants. The same viewpoint also underpins modern learning-based systems, where we often fuse intermediate features rather than raw measurements. In this chapter, we first introduce a taxonomy of fusion modes and architec- tures, then develop Bayesian fusion using linear and nonlinear filters, includ- ing bias-aware modeling and conservative strategies for unknown correlations. We discuss practical issues such as spatial registration, time alignment, multi- rate/asynchronous updates, and out-of-sequence measurements. We then con- nect fusion to object tracking, covering gating and data association (GNN/JP- DA/MHT) as well as advanced random finite set (RFS) methods, and finally sensor fusion and object tracking 393 survey modern practice in feature- and BEV-level fusion, uncertainty calibration for learned modules, and cooperative (V2X) perception. Throughout, we tie the abstractions back to real robotic systems using examples like those above. 15.1 A Taxonomy of Sensor Fusion We begin by situating sensor fusion along three complementary axes: one de- scribes the data we receive from sensors, one characterizes the fusion problem, and one describes the system architecture. Thinking through all three lenses helps identify both an appropriate mathematical formulation and the right engineering design choices for a given application. Data-related taxonomy. Real sensors are imperfect in several simultaneous ways. Measurements are uncertain due to stochastic noise, biased because of drifts and miscalibrations, and coarse due to finite resolution or quantization 5 . Beyond 5 M. Liggins I, D. Hall, and J. Llinas. Handbook of multisensor data fusion: theory and practice. CRC press, 2017 these first-order issues, real systems often exhibit: âą Correlations across streams, for example due to shared timing, mounting, vi- brations, or processing pipelines. âą Disparities between modalities, such as different range envelopes and failure modes. âą Outright inconsistencies, such as outliers, contradictory readings, or disorder due to out-of-sequence arrivals. Robust fusion methods must explicitly acknowledge these realities. In this chap- ter, we will model noise and bias (Section 15.2.1), guard against cross-stream double-counting when correlations are unknown (Section 15.2.2), and handle asynchrony and delayed packets with fixed-lag smoothing (Section 15.3.4). Fusion-related taxonomy. It is also useful to classify problems by what we fuse. At the lowest level, we combine raw or lightly processed time series such as range readings or pixel measurements. At an intermediate level, we fuse features or representations such as birdâs-eye-view (BEV) representations or learned embed- dings. At a high level, we fuse decisions such as detections, tracks, or maps. These are often termed early, mid, and late fusion. All three appear in practice and impose different constraints on bandwidth, latency, and calibration. Orthogonal to the representational choice is the relationship among sensors, leading to competitive fusion, complementary fusion, and cooperative fusion. In com- petitive fusion, redundant measurements of the same quantity are combined to improve reliability via consistency checks. In complementary fusion, we combine sensors that see different aspects of the world, such as lidar for short-range ge- ometry and radar for long-range motion, to fill in gaps and reduce ambiguity. In cooperative fusion, sensors recover information that no single one can provide 394 principles of robot autonomy alone, as when monocular vision acquires metric scale only when paired with inertial sensing 6 . 6 For example, GNSS localization and stereo vision can be cooperatively fused because they measure fundamentally different properties of the environment. In this chapter, we give each of these a probabilistic interpretation, treating every stream as a stochastic observation, and show how the choice of level and relationship guides the appropriate update rules and statistical tests (Sections 15.2 and 15.5). Architectural taxonomy. Finally, we classify fusion by where the computation takes place. In a centralized architecture, raw data or features are transported to a fusion center that estimates the state using all information at once. This is statistically efficient but demands bandwidth and creates single points of failure. In a de- centralized architecture, each platform or subsystem runs a local estimator and transmits summaries, such as tracks with covariances, to a higher layer that fuses posterior distributions. This reduces raw bandwidth but raises questions about cross-correlation. In a distributed architecture, peers exchange beliefs or innova- tions over a communication graph and seek agreement via consensus or infor- mation sharing. Such architectures scale naturally to vehicle-to-everything (V2X) and multi-robot scenarios but must preserve consistency when cross-covariances are unknown. We will see how information-form filters support decentralized and distributed updates, and how conservative schemes such as Covariance Inter- section maintain correctness in the face of unknown dependencies (Section 15.4) 7 . 7 R. Olfati-Saber, J. A. Fax, and R. M. Murray.âConsensus and Cooperation in Networked Multi-Agent Systemsâ. In: Proceedings of the IEEE 95.1 (2007), p. 215â233 These three taxonomiesâimperfections in the data, the level and relationship of fusion, and the architecture of computationâprovide a scaffold for the remainder of the chapter. We move from Bayesian formulations and Kalman-family up- dates to the practicalities of registration and asynchrony, then on to multi-object tracking and modern feature/BEV-level fusion, referring back to this taxonomy to justify design choices along the way. 15.2 Bayesian Approach to Sensor Fusion In previous chapters, we introduced Bayes-filter-based algorithms for state esti- mation and localization. The very same viewpoint naturally solves sensor fusion: each sensor contributes a probabilistic observation of the latent state, and Bayesâ rule combines these contributions into a posterior belief that is more precise and more reliable. Beliefs, likelihoods, and Bayesâ rule. We model unknown quantities as random vari- ables and represent knowledge as probability distributions. Let x t denote the latent state at time t and let Z t = z (i) t m i=1 be the set of measurements from m sensors at that time. Under the assumption that the measurements are condition- ally independent given x t , the Bayesian update from prior p(x t ) is: p(x t |Z t )â p(x t ) m â i=1 p(z (i) t | x t ).(15.1) sensor fusion and object tracking 395 If this conditional independence assumption does not hold, we must either model the joint likelihood (including cross-covariances) or use conservative fusion strate- gies that remain valid under unknown correlations (Section 15.2.2). In either case, the product of likelihoods in Equation (15.1) formalizes the intuition that multiple sensors together reduce uncertainty. Why the Bayesian approach? The Bayesian viewpoint is attractive for several rea- sons: âą It provides a unified, interpretable representation of information: any modality that can be probabilistically modeled can be expressed as a likelihood over the state. âą It natively handles uncertainty 8 . 8 For example, the variance of a Gaus- sian posterior quantifies dispersion. Cal- ibrated posteriors therefore allow princi- pled gating and fault detection. âą It offers a principled update rule via Bayesâ theorem and naturally handles missing data, delayed packets, and novel observations. Example 15.2.1 (Probabilistic competitive fusion). Consider two sensors measur- ing the same scalar quantity x â R, producing measurements z 1 and z 2 with Gaussian noise: p(z 1 | x) =N (z 1 ; x, Ï 2 1 ),p(z 2 | x) =N (z 2 ; x, Ï 2 2 ). Treating the likelihood as a function of x, their product is proportional to a Gaus- sian in x: p(z 1 , z 2 | x)â N x; ÎŒ, Ï 2 ,ÎŒ = z 1 Ï 2 2 + z 2 Ï 2 1 Ï 2 1 + Ï 2 2 ,Ï 2 = Ï 2 1 Ï 2 2 Ï 2 1 + Ï 2 2 . The maximum-likelihood estimate of x is therefore a precision-weighted average of z 1 and z 2 , and the joint uncertainty strictly decreases: Ï 2 < minÏ 2 1 , Ï 2 2 . If we also incorporate a Gaussian prior x ⌠N (ÎŒ 0 , Ï 2 0 ), the posterior remains Gaussian with: Ï â2 post = Ï â2 0 + Ï â2 1 + Ï â2 2 ,ÎŒ post = Ï 2 post ÎŒ 0 Ï 2 0 + z 1 Ï 2 1 + z 2 Ï 2 2 , and the earlier result is recovered in the uninformative-prior limit Ï 2 0 ââ. 15.2.1 Linear-Gaussian Fusion: The Kalman Filter The Kalman filter, introduced in Chapter 12, is the linear-Gaussian instance of the Bayesian filter and a workhorse for sensor fusion. We assume linear dynamics: x t = A t x tâ1 + B t u t + Δ t ,Δ t âŒN (0, Q t ), and a linear measurement model: z t = C t x t + ÎŽ t ,ÎŽ t âŒN (0, R t ), with a Gaussian belief bel(x t ) ⌠N (ÎŒ t ,ÎŁ t ). Here Q t is the process-noise covari- ance and R t is the measurement-noise covariance, which may encode multiple sensors. 396 principles of robot autonomy Prediction update. The Kalman filter prediction step is: ÌÎŒ t = A t ÎŒ tâ1 + B t u t , Ì ÎŁ t = A t ÎŁ tâ1 A †t + Q t , where ÌÎŒ t and Ì ÎŁ t denote the predicted mean and covariance at time t. Measurement update (stacked multi-sensor form). Suppose m sensors report at time t. We stack their measurements into a single vector: z t = ïŁź ïŁŻ ïŁŻ ïŁ° z (1) t . . . z (m) t ïŁč ïŁș ïŁș ïŁ» ,C t = ïŁź ïŁŻ ïŁŻ ïŁ° C (1) t . . . C (m) t ïŁč ïŁș ïŁș ïŁ» ,R t = blkdiag R (1) t , . . . , R (m) t , where C (i) t and R (i) t are the measurement matrix and noise covariance for the i-th sensor. Then: Ìz t = z t â C t ÌÎŒ t ,S t = C t Ì ÎŁ t C †t + R t ,K t = Ì ÎŁ t C †t S â1 t , and the update becomes: ÎŒ t = ÌÎŒ t + K t Ìz t ,ÎŁ t = (Iâ K t C t ) Ì ÎŁ t . Sensors with smaller covariance in R t are automatically weighted more heavily in the Kalman gain 9 . 9 This is visible in K t = Ì ÎŁ t C †t (C t Ì ÎŁ t C †t + R t ) â1 . Information form (useful for decentralized/distributed fusion). Define the information matrix and vector by Y t =ÎŁ â1 t and y t = Y t ÎŒ t . Each independent measurement block contributes: âY (i) = C (i)†t R (i)â1 t C (i) t ,ây (i) = C (i)†t R (i)â1 t z (i) t . Fusion then reduces to summation: Y + = Y â + â i âY (i) ,y + = y â + â i ây (i) . This additive structure makes it natural to combine multi-sensor updates or ex- change increments over a network, as we will discuss in Section 15.4. Innovation tests and gating. Define the innovation Ìz t and its covariance S t as above. The Normalized Innovation Squared (NIS): NIS = Ìz †t S â1 t Ìz t , follows a Ï 2 distribution of appropriate dimension under correct modeling. We will use this to gate outliers 10 , detect faults, and monitor consistency. 10 We revisit gating in Section 15.5.2. sensor fusion and object tracking 397 Augmenting the state (bias-aware fusion). A powerful trick is to augment the state x with nuisance parameters such as sensor biases or calibration terms. For an additive bias b, we define the augmented state: x âČ = " x b # ,b t = b tâ1 + w t , w t âŒN (0, Q b ), and a measurement z t = Cx t + b t + ÎŽ t becomes linear in x âČ with measurement matrix [C I]. This allows the filter to learn biases online and prevents them from masquerading as state errors. Scale or misalignment biases can be handled similarly with appropriate parameterizations. Nonlinear sensors: EKF and UKF. For nonlinear measurements z t = h(x t ) + ÎŽ t , we can either linearize h via the Jacobian H t = âh âx ÎŒ t and apply the extended Kalman filter (EKF), or propagate sigma points through h in the unscented Kalman filter (UKF). Both preserve the Bayesian structure, and the UKF generally performs better on strongly nonlinear problems. Bias augmentation and stacked updates carry over unchanged. Example 15.2.2 (Kalman filter multi-sensor fusion). Consider a self-driving car equipped with an IMU, a GNSS receiver, and a lidar sensor. We estimate lon- gitudinal position p, velocity v, and acceleration a using a constant-acceleration kinematic model: Ì p = v, Ì v = a. Discretizing with sampling time T and allowing process noise Δ t ⌠N (0, Q t ) gives ïŁź ïŁŻ ïŁ° p t+1 v t+1 a t+1 ïŁč ïŁș ïŁ» = ïŁź ïŁŻ ïŁ° 1T T 2 2 01T 001 ïŁč ïŁș ïŁ» |z A ïŁź ïŁŻ ïŁ° p t v t a t ïŁč ïŁș ïŁ» + Δ t . Suppose lidar and GNSS measure the position p, and the IMU measures the acceleration a: ïŁź ïŁŻ ïŁ° z lidar,t z gnss,t z imu,t ïŁč ïŁș ïŁ» = ïŁź ïŁŻ ïŁ° 1 0 0 1 0 0 0 0 1 ïŁč ïŁș ïŁ» | z C ïŁź ïŁŻ ïŁ° p t v t a t ïŁč ïŁș ïŁ» + ÎŽ t , ÎŽ t âŒN 0, R t = ïŁź ïŁŻ ïŁ° Ï 2 lidar 00 0Ï 2 gnss 0 00Ï 2 imu ïŁč ïŁș ïŁ» . Here R t stacks the sensor variances, Ï 2 lidar , Ï 2 gnss , and Ï 2 imu . Example 15.2.2 shows that adding a lower-variance GNSS channel tightens the position estimate and reduces noise 11 . The relative weighting is handled automatically by the Kalman 11 Even a noisier additional sensor can help by improving observability or by providing redundancy for fault detec- tion, but the effect is more modest. gain. 15.2.2 Unknown Correlations and Conservative Fusion When fusing estimates from separate filters 12 , cross-covariances are often un- 12 For example, in track-to-track fusion, decentralized networks, or when reusing map priors. 398 principles of robot autonomy Figure 15.2: Kalman filter sen- sor fusion for Example 15.2.2. The position of a vehicle is esti- mated using noisy lidar, GNSS, and IMU data, and the result- ing estimate tracks the ground truth. Adding the GNSS sensor improves the estimate through fusion. known. Naively fusing them as if they are independent produces overconfident posteriors. A principled, conservative alternative is Covariance Intersection (CI), which combines Gaussian estimates without requiring cross-covariances. Given (ÎŒ 1 ,ÎŁ 1 ) and (ÎŒ 2 ,ÎŁ 2 ), CI defines: ÎŒ CI =ÎŁ CI ÏÎŁ â1 1 ÎŒ 1 + (1â Ï)ÎŁ â1 2 ÎŒ 2 , ÎŁ â1 CI = ÏÎŁ â1 1 + (1â Ï)ÎŁ â1 2 , with Ï â [0, 1] chosen to minimize, for example, tr(ÎŁ CI ) or log detÎŁ CI . CI guar- antees consistency irrespective of the true, unknown correlation structure and will reappear in our discussion of distributed fusion in Section 15.4. In summary, the Bayesian lens turns the problem of sensor fusion into a precise algebra over beliefs and likelihoods. Linear-Gaussian assumptions yield closed- form Kalman filters with simple stacked updates and an additive information form, mild nonlinearities are handled by EKF/UKF variants, and practical wrin- kles such as biases, asynchrony, and unknown correlations are addressed with state augmentation, smoothing, and conservative fusion. We build on this foun- dation in the next sections. sensor fusion and object tracking 399 15.3 Engineering Realities: From Models to Working Systems The Bayesian rules from the previous section tell us how to combine beliefs and likelihoods. Real robots, however, add a layer of practical complexity: sensors are mounted somewhere on the body and point in particular directions, they operate at different rates with different latencies, networks drop or reorder packets, and multiple streams may be subtly correlated. This section bridges the gap between the clean algebra of Bayesâ rule and systems that survive contact with hardware. We move from where a sensor lives (spatial registration), to when it speaks (time alignment and multi-rate updates), to what happens when it speaks late (out- of-sequence measurements), and finally to staying honest about information (un- known correlations and faults). 15.3.1 Spatial Registration and Calibration Before any fusion can succeed, measurements must be made commensurate with per-sensor intrinsics 13 and extrinsics 14 . Calibration is not a one-time event: tem- 13 Such as a cameraâs focal length and distortion, an IMUâs scale factors and axis misalignment, or a radarâs range/- Doppler scaling. 14 The rigid transform from each sensor frame to the body frame and, where needed, from the body to a global frame. perature changes, mechanical wear, and firmware updates can shift sensor pa- rameters over time. A filter that appears increasingly overconfident often points to registration that has drifted out of spec. A useful way to understand the effect of small miscalibration is to linearize it. If a measurement depends on the transform from body B to sensor S, denoted S T B , through some function h, then around a nominal S Ì T B we can write: z â h S Ì T B x + J T η + ÎŽ, where η is a small pose perturbation, J T is its sensitivity, and ÎŽ is sensor noise. This first-order view suggests two complementary strategies: âą Treat extrinsics as known but uncertain: inflate the measurement covariance by J T ÎŁ T J †T , whereÎŁ T encodes extrinsic uncertainty. This preserves consistency without changing the state. âą Augment the state with a minimal parameterization of the extrinsic and assign it a slow random-walk prior. The filter then learns small misalignments online rather than forcing other states to absorb them. Example 15.3.1 (Online cameraâlidar extrinsics). A mobile platform relies on a pre-calibrated extrinsic transform cam T lidar . Over weeks, thermal drift subtly increases the innovation statistics (NIS) of the camera measurements. By aug- menting the filter state with a six-degree-of-freedom perturbation of cam T lidar and adding sparse visualâdepth correspondences as pseudo-measurements, the sys- tem recenters the alignment online. A small process noise on the extrinsic states prevents overfitting transient effects. 400 principles of robot autonomy 15.3.2 Time Alignment, Latency, and Stamps vs. Arrival Sensors almost never report in lockstep time. They are clocked by different oscil- lators, traverse different processing pipelines, and communicate through different buses. The filter, however, should apply each measurement at the time it was taken. Accurate measurement timestamps are therefore critical; arrival times are, at best, indirect clues. Hardware triggers or disciplined clocks, such as those using Network Time Protocol (NTP) or Precision Time Protocol (PTP), reduce relative drift. Addition- ally, known fixed latencies, such as a cameraâs ISP delay, should be subtracted so that time stamps reflect exposure times, not the end of the processing chain. When an update is due at an intermediate time Ï between filter steps, the state should be interpolated to Ï before applying the measurement. For inertial nav- igation, IMU data can be preintegrated over (t i , t j ] to produce a relative-motion pseudo-measurement consistent with SE(3) geometry, removing the need to re- sample at the IMU rate. Example 15.3.2 (Stamp vs. arrival time matters). A radar packet arrives at t = 1.030 s with a timestamp Ï = 1.000 s. If the filter treats the packet as if it were taken at 1.030 s, the update is effectively âtime-shifted,â biasing the estimate and corrupting the covariance. Maintaining a short, time-ordered buffer of states and applying the update at Ï avoids this problem and sets the stage for proper handling of genuinely late packets in Section 15.3.4. 15.3.3 Multi-Rate and Asynchronous Updates On a typical platform, the IMU runs at hundreds of Hertz, cameras at a few tens, lidar somewhere in between, and GNSS in single digits. The Kalman family naturally accommodates this: we predict forward to the next measurement time, and we update whenever a packet arrives. In software, it is convenient to maintain a priority queue keyed by measure- ment time. The filter repeatedly advances (predicts) to the earliest timestamp, next applies 15 all measurements with that timestamp, and then repeats. This sim- 15 Stacking multiple measurements with the same stamp into a single block so that the innovation covariance S t reflects their joint effect. ple disciplineââpredict to the stamp, then stack at the stampââprevents subtle double counting and preserves the meaning of NIS tests used later. 15.3.4 Out-of-Sequence Measurements and Fixed-Lag Smoothing Even with careful time stamping, networks reorder and delay packets. A classical example is GNSS delivered over a congested channel, where a position measure- ment computed at time kâ â may arrive at time k. Applying the measurement at the head of the filter timeline warps the uncertainty and can cause visible jumps in the estimate. The remedy is to keep a short history and re-solve the portion of the problem that lies within that window. A high level procedure for fixed-lag RauchâTungâStriebel (RTS) 16 smoothing for out-of-sequence measurements is: 16 H. E. Rauch, F. Tung, and C. T. Striebel. âMaximum likelihood estimates of lin- ear dynamic systemsâ. In: AIAA journal 3.8 (1965), p. 1445â1450 sensor fusion and object tracking 401 1. Fix a lag L and maintain a buffer of estimatesÎŒ j|j ,ÎŁ j|j k j=kâL together with the dynamics (A, Q). Suppose a measurement (z kââ , C, R) with 0†â†L arrives. 2. Run an RTS backward pass from k back to kâ â to compute smoothed estimates ÎŒ j|k ,ÎŁ j|k (and, if needed, cross-covariances). 3. Insert the update at kâ â using ÎŒ kââ|k as the prior. 4. Re-propagate forward from kâ â to k, re-applying any later measurements in the buffer. 5. Commit the corrected states. The lag L should exceed typical delay jitter. If a packet falls outside the win- dow, we can either discard it or assimilate it approximately with an inflated, time-shifted update. Example 15.3.3 (GNSS delay in a lidarâIMU EKF). An EKF runs at IMU rate (200 Hz) with lidar odometry updates at 10 Hz. GNSS packets occasionally arrive 300 ms late. With a fixed lag L = 1 s, the filter back-smooths, inserts the GNSS update at its true time, and re-propagates. The NIS distribution tightens, and the trajectory becomes free of the jumps that were present when late packets were applied at the head of the queue. 15.3.5 Unknown Correlations, Double Counting, and Consistency Fusion is not only about more data, it is about honest information. Measure- ment streams may be correlated because they share process noise 17 , because they 17 For example, the sensors are mounted on the same body. reuse the same map or prior, or because one stream already incorporates mea- surements from the other. If cross-covariances are known, they can be modeled explicitly. Often they are not, and fusing as if streams were independent yields overconfident covariances: NIS and NEES statistics will then fail Ï 2 checks. Several responses are available: âą When possible, exchange innovations (residuals and their covariances) rather than full posteriors, since innovations are closer to independent across nodes. âą When that is insufficient, conservative fusion methods such as Covariance In- tersection (Section 15.2.2) combine Gaussians without any knowledge of cross- covariances and guarantee consistency at the price of modest optimality. âą In all cases, monitor empirical NIS/NEES against theoretical quantiles and treat persistent deviations as feedback that some correlation, timing error, or noise model has been overlooked. 15.3.6 Fault Detection and Isolation Even a well-registered, well-timed system must protect itself against bad data. Kalman filters already provide a convenient diagnostic: the innovation Ìz and 402 principles of robot autonomy its covariance S define the NIS Ìz †S â1 Ìz, which under a correct model follows a Ï 2 distribution of known dimension. Setting a gate at a chosen significance level rejects gross outliers, and tracking moving averages of the NIS per sensor exposes slow degradation. When a sensor begins to misbehave, there are two complementary reactions: âą Innovation-based adaptive estimation: inflate that sensorâs measurement covari- ance R according to a feedback law derived from recent NIS so that the filter automatically down-weights it. âą Structural response: if the issue resembles a bias or scale error more than ran- dom noise, augment the state with the offending parameter and let the filter learn it. In systems with redundancy 18 , it is natural to maintain a per-sensor health score 18 Two or more sensors nominally mea- suring the same quantity. and to prefer the healthier stream while keeping the others alive for fault detec- tion and graceful recovery. Example 15.3.4 (Rain on a camera, radar nominal). During a storm, the cameraâs measurements intermittently fail the NIS gate, while radar remains nominal. The system inflates the cameraâs measurement covariance R and leans more heavily on radar for range and velocity. Once the rain clears and the cameraâs NIS returns to expected quantiles, its weight rises automatically. Throughout, the reported covariance remains conservative, so downstream planners are not surprised. 15.3.7 Numerical Stability and Practical Monitors A few numerical practices are particularly useful. First is the use of square-root filters, which propagate a Cholesky factor ofÎŁ to be more stable than propagat- ingÎŁ directly, and help prevent negative-definite covariances due to round-off. Second, when updating the covariance we can use the Joseph stabilized form: ÎŁ t = (Iâ K t C t ) Ì ÎŁ t (Iâ K t C t ) †+ K t R K †t , which preserves positive semidefiniteness in finite precision. On the monitoring side, plotting NIS/NEES histograms with Ï 2 overlays, tracking the determinant and condition number ofÎŁ, and logging gate hit-rates and stamp-minus-arrival statistics provide early warnings for timing and calibration regressions. Takeaway. Registration and timing turn raw measurements into commensurate ev- idence; buffering and smoothing reconcile the past with the present; and attention to correlation and faults keeps the estimator honest. With these pieces in place, we can extend from single-state fusion to the challenges of object tracking and the demands of distributed systems, which we address next. sensor fusion and object tracking 403 15.4 Fusion Architectures The Bayesian viewpoint tells us how evidence should be combined and the archi- tecture decides where the combination happens and what is exchanged. In prac- tice, three recurring patterns emerge: centralized systems that gather everything in one place, decentralized systems that fuse local tracks at a higher layer, and dis- tributed systems that reach agreement over a network. Each involves trade-offs in bandwidth, latency, robustness, and the ease of maintaining consistent uncertainty (Section 15.3.5). 15.4.1 Centralized Fusion Centralized designs are conceptually simplest: raw measurements or lightly pro- cessed features are transported to a single estimator that maintains the posterior over the state. Mathematically, this is just the stacked update in Section 15.2.1, where at time t we form: z t = ïŁź ïŁŻ ïŁ° z (1) t . . . z (m) t ïŁč ïŁș ïŁ» ,C t = ïŁź ïŁŻ ïŁ° C (1) t . . . C (m) t ïŁč ïŁș ïŁ» ,R t = blkdiag R (1) t , . . . , R (m) t , compute the innovation Ìz t and its covariance S t , and apply a single Kalman up- date. Centralized fusion is statistically efficient and gives the clearest path to proper gating, bias handling, and OOSM smoothing (Sections 15.3.2 and 15.3.4). Its limitations are operational rather than mathematical. Communicating raw sensor streams consumes bandwidth, creates central bottlenecks and single points of failure, and can be awkward when sensors live on different platforms, such as across vehicles and infrastructure. A common compromise is to centralize only within a platform or robot, fusing camera/lidar/radar/IMU locally and exposing higher-level artifactsâdetections, tracks, occupancyâto the outside world. 15.4.2 Decentralized Track-to-Track Fusion In a decentralized system, each subsystem or platform runs its own filter and publishes summarized beliefsâtypically a state estimate with covariance and, possibly, a timestamp and a health score. A fusion node then combines these âtracksâ 19 . The main question is how to fuse them without breaking consistency. 19 S. Zhang, Y. Bar-Shalom, and G. Wat- son.âTracking with multisensor out- of-sequence measurements with residual biasesâ. In: 2010 13th International Con- ference on Information Fusion. IEEE. 2010, p. 1â8 If the contributing tracks are conditionally independent given the true state 20 , 20 For example, if they use disjoint raw measurements. fusion is straightforward in information form. If a local node transforms a common prior (Y â , y â ) into (Y + , y + ), it can transmit its increment: âY ⥠Y + â Y â ,ây⥠y + â y â , and the fusion node simply adds the increments from all sources: (Y, y)â (Y â , y â ) + â i (âY (i) ,ây (i) ). 404 principles of robot autonomy In the linear-Gaussian case, these increments equal C †R â1 C and C †R â1 z for that nodeâs local measurements. This âcommon-prior + incrementâ view keeps the algebra exact, but it requires that all nodes agree on (or communicate) the prior against which those increments were formed. When two tracks are not independentâbecause they share process noise, maps, or each otherâs measurementsâtheir cross-covariance is generally unknown. Fus- ing as if independent is then dangerous (Section 15.3.5). A safe default is Co- variance Intersection (Section 15.2.2), which guarantees consistency at the cost of some conservatism 21 . If cross-covariances are known 22 , we can compute the op- 21 S. J. Julier and J. K. Uhlmann. âA non- divergent estimation algorithm in the presence of unknown correlationsâ. In: Proceedings of the 1997 American Control Conference (Cat. No. 97CH36041). Vol. 4. IEEE. 1997, p. 2369â2373 22 For example, in a carefully engineered multi-radar system with a common pro- cess model. timal best linear unbiased estimator (BLUE) weights using the joint covariance of the two track estimates. In practice, the engineering overhead to maintain those cross-terms often outweighs the marginal gain over CI. Example 15.4.1 (Track-to-track fusion with equivalent information). Two drones estimate a shared targetâs position. Each runs a local EKF and, at each second, exports the pair (âY,ây) computed from its local pre- and post-update informa- tion states. A ground station maintains a common prior and updates it by addition. Because increments are additive and tied to a common prior, the result matches exactly what would have been obtained had the ground station received both raw measurement streams centrally. If a communications hiccup delays one droneâs packet, the ground station treats it as an out-of-sequence increment and inserts it using the same fixed-lag machinery as in Section 15.3.4. 15.4.3 Distributed Fusion by Consensus and Information Exchange Truly distributed systems have no fusion center. Instead, peers exchange mes- sages over a communication graph and attempt to agree on the posterior. The information form makes this natural: at each time step, node i computes its local increment (âY i ,ây i ) from its own measurements, then the network mixes these contributions so that everyone converges to the same sum. One simple version is average consensus on information increments using a sym- metric, doubly stochastic mixing matrix W = [w ij ] that respects the communi- cation graph. Assume all nodes start from the same prior (Y â , y â ). Each node initializes its local consensus state from its own increment: Ì âY (0) i =âY i , Ì ây (0) i =ây i . Then perform r rounds of neighbor averaging: Ì âY (s+1) i = â j w ij Ì âY (s) j , Ì ây (s+1) i = â j w ij Ì ây (s) j (s = 0, . . . , râ 1). When the graph is connected and W is well chosen, Ì âY (s) i and Ì ây (s) i converge to the network-wide averages of the increments. An exact centralized posterior is then recovered by multiplying by the number of nodes n: Y + i = Y â + n Ì âY (r) i ,y + i = y â + n Ì ây (r) i . sensor fusion and object tracking 405 In directed or time-varying graphs, push-sum or diffusion variants play the same role. In practice, a small number of rounds 23 often captures most of the benefit. 23 Even r = 1 or 2. Correlation again requires care. If nodes are connected through shared pro- cess models or re-used features, repeated neighbor mixing can re-inject the same evidence multiple times. Two mitigations are common. First, we can exchange innovations rather than full posteriors, since these tend to be closer to indepen- dent across nodes. Second, we can replace consensus averaging with CI-consensus: combining neighborsâ information conservatively when independence is doubtful so each nodeâs covariance remains an upper bound on its true error. Algorithm 15.1: Consensus information filtering (node i). Data: Common prior (Y â , y â ), local measurementsz i givingâY i ,ây i , neighbor setN (i), mixing weightsw ij with â j w ij = 1, number of nodes n, number of rounds r. Result: Posterior (Y + i , y + i ). // Initialize Ì âY i ââY i Ì ây i âây i for k = 1 to r do Get ( Ì âY j , Ì ây j ) from neighbor node jâN (i) Ì âY i â â j w ij Ì âY j Ì ây i â â j w ij Ì ây j Y + i â Y â + n Ì âY i y + i â y â + n Ì ây i 15.4.4 What to Send: Raw Data, Features, or Tracks? Architectures are inseparable from communication budgets. A useful mental model is to choose a bandwidth tier: âą Raw or ROI data 24 : maximal accuracy and flexibility, but highest bandwidth, 24 For example, point clouds or image crops. strict latency requirements, and potential privacy concerns. âą Features or birdâs-eye-view (BEV) grids: a strong trade-off in modern stacks; se- mantics are preserved, bandwidth is moderate, and time alignment can be handled at the feature level. âą Decisions (detections, tracks, occupancy): minimal bandwidth and simplest to distribute; best suited to decentralized and distributed fusion, but with the least flexibility for correcting upstream errors. Two additional practicalities: âą Messages should carry uncertainty (covariances or credible intervals), not just point estimates, to enable principled fusion at the receiver. 406 principles of robot autonomy âą Timing matters as much as content: include measurement timestamps, not just send times, so that receivers can place information correctly on their own timelines (Sections 15.3.2 and 15.3.4). Example 15.4.2 (Cooperative perception at an intersection). Consider an urban intersection with an instrumented roadside unit (RSU) and vehicles approaching from multiple directions. Each vehicle maintains a centralized, on-board fusion stack (camera/lidar/radar/IMU) and publishes a stream of tracks with covari- ances. The RSU runs its own perception stack from elevated cameras and a 4D imaging radar. A decentralized fusion server aggregates vehicle and RSU tracks via equivalent information increments tied to a common prior at 10 Hz. Vehicles subscribe to this fused track set. When bandwidth is plentiful, the RSU also publishes mid-level BEV features for regions of interest such as crosswalks. Nearby vehicles that can spare compute perform a short, two-round consensus step (Algorithm 15.1) on the BEV-derived information, which sharpens occupancy in occluded regions. During peak con- gestion, the system falls back to track-level CI fusion to preserve consistency un- der stronger correlations. The result is a layered architecture: centralized fusion within each agent, decentralized fusion at the server for robustness, and briefly distributed fusion among peers when conditions allow. Takeaway. Centralized fusion is the gold standard when bandwidth and com- pute permit it. Decentralized fusion scales well and is straightforward when independence holds (or CI is used when it does not). Distributed fusion achieves resilience and coverage across a network, provided that messages preserve tim- ing, carry uncertainty, and respect the difference between independence and cor- relation. We now turn from what and where to fuse to the closely related problem of tracking multiple objects through time. 15.5 Object Tracking Sensor fusion becomes especially important when the goal is not just an ego state but a changing population of objects: vehicles, pedestrians, drones, or landmarks that appear, move, occlude one another, and disappear. The task of object track- ing is to estimate, through time, both the continuous states, such as positions and velocities, and the discrete identities of these objects given noisy, partial, and sometimes contradictory measurements. In this section, we build a practical tracker from motion and measurement models, add principled gating to keep outliers at bay, address data association to decide which detection belongs to which track, and define track management to handle births, deaths, and occlusions. We close with an advanced view based on random finite sets and a brief tour of tracking-by-detection systems and evalua- tion metrics. sensor fusion and object tracking 407 15.5.1 States, Motion Models, and Measurements We model each target k at time t with a state vector x k t and a measurement model that connects states to observed quantities. A common starting point for ground vehicles is a constant-velocity (CV) model in the plane: x t = h xy Ì x Ì y i †,F(ât) = ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁ° 1 0ât0 0 10ât 0 010 0 001 ïŁč ïŁș ïŁș ïŁș ïŁ» , with white-acceleration process noise: Q(q,ât) = q ïŁź ïŁŻ ïŁŻ ïŁŻ ïŁŻ ïŁ° ât 3 3 0 ât 2 2 0 0 ât 3 3 0 ât 2 2 ât 2 2 0ât0 0 ât 2 2 0ât ïŁč ïŁș ïŁș ïŁș ïŁș ïŁ» . For targets that turn, a coordinated-turn (CT) model augments the state with a turn rate Ï and typically uses an EKF or UKF update. A constant-acceleration (CA) model can be more appropriate for pedestrians or stop-and-go traffic. The choice of model trades fidelity for simplicity; a good practice is to begin with CV and only move to CT or CA if innovation statistics suggest a benefit. Measurements take many forms. A radar may report range r, bearing Ï, and sometimes radial velocity Ì r: h radar (x) = ïŁź ïŁŻ ïŁŻ ïŁ° p x 2 + y 2 atan2(y, x) x Ì x+y Ì y â x 2 +y 2 ïŁč ïŁș ïŁș ïŁ» + ÎŽ, ÎŽâŒN (0, R). Camera detections may provide image-space bounding boxes and, if depth is available, 3D centroids in the ego frame. Lidar clusters similarly yield centroids and sometimes yaw estimates. We fold these into linear or linearized measure- ment models and apply the Kalman-family update from Section 15.2.1 for each track. 15.5.2 Gating and the Role of the Innovation Before tackling the combinatorics of association, we narrow the search space with gating. Given a predicted measurement Ìz for a track and its innovation covariance S, a detection z is deemed compatible if the (squared) Mahalanobis distance: m 2 (z) = (zâ Ìz) †S â1 (zâ Ìz), lies below a threshold from the Ï 2 distribution at a chosen confidence 25 . This is 25 For example, α = 0.99 for a conserva- tive, wide gate. exactly the NIS concept from Section 15.3; under the model, m 2 follows Ï 2 d for measurement dimension d. Gating dramatically reduces the clutter presented to the assignment solver: it retains almost all true matches while excluding most spurious ones. 408 principles of robot autonomy Example 15.5.1 (Ellipsoidal gates in practice). With radar rangeâbearing measure- ments (d = 2), a threshold near Ï 2 2,0.99 â 9.21 carves out an ellipse in measure- ment space. In dense traffic, this can reduce candidates per track from dozens to a handful, turning an intractable assignment into a manageable one while pre- serving nearly all true associations. 15.5.3 Data Association: From Nearest Neighbor to Probabilistic After gating, we must decide which detection updates which track. The sim- plest approach is Global Nearest Neighbor (GNN): we define a cost matrix C ij be- tween track i and detection j (often using Mahalanobis distance), and solve the 2-D assignment problem with the Hungarian algorithm. Algorithm 15.2 summa- rizes a standard gated GNN pipeline with gating and occlusion handling. Unas- signed tracks receive missed-detection events; unassigned detections may seed new tracks. GNN is fast and effective in relatively unambiguous scenes, but it commits to a single hypothesis even when multiple associations are plausible. Two families of methods hedge this ambiguity. Probabilistic (joint) data association (PDA/JPDA). In Probabilistic Data Association (PDA), each track i considers all detections in its gate and performs a mixture update weighted by association probabilities ÎČ ij and a missed-detection prob- ability ÎČ i0 . Joint PDA (JPDA) generalizes this to multiple tracks, computing a consistent set of probabilities across the scene. For a track with prior (ÎŒ â ,ÎŁ â ) and each candidate detection j, we compute the innovation Ìz j , its likelihood: L j â exp â 1 2 Ìz †j S â1 Ìz j â det S , and then normalize (with clutter intensity included) to obtain the ÎČ ij . For each candidate association j, let (ÎŒ + j ,ÎŁ + j ) denote the conditional KF/EKF posterior, and let (ÎŒ + 0 ,ÎŁ + 0 ) = (ÎŒ â ,ÎŁ â ) denote the missed-detection case. JPDA then matches the first two moments of this mixture: ÎŒ + = M â j=0 ÎČ ij ÎŒ + j , ÎŁ + = M â j=0 ÎČ ij h ÎŁ + j + (ÎŒ + j â ÎŒ + )(ÎŒ + j â ÎŒ + ) †i . The second term inside the brackets captures residual association ambiguity, such that even if each conditional posterior is sharp, disagreement among the candi- date means keeps the final covariance honest. JPDAâs main cost is combinatorial in the worst case, but clustering or pruning unlikely joint events keeps it tractable. sensor fusion and object tracking 409 Multiple-hypothesis tracking (MHT). Multiple-hypothesis tracking maintains a small forest of competing association histories and prunes them by likelihood. Each hy- pothesis carries its own set of track states. When new detections arrive, the tree branches over plausible assignments; pruning and N-scan backtracking keep the tree shallow. Deferred decisions across a short horizon can significantly improve performance in crowded, ambiguous scenes. Algorithm 15.2: GNN with gating and occlusion handling. Data: Predicted tracksÎŒ i ,ÎŁ i , detectionsz j , gating threshold Ï, cost function C ij . Result: Updated tracks, with occluded ones kept alive via motion-only prediction for a limited horizon. for each track i do Compute Ëz i , S i , and gate detections by m 2 †Ï. for each detection j do Build C ij , e.g., set C ij = m 2 ij for gated pairs and a large cost otherwise. Solve the 2-D assignment with the Hungarian algorithm. Update matched tracks via KF/EKF. For unmatched tracks, perform a missed-detection update (increase uncertainty, decrease a survival score). For unmatched detections, run initiation logic (Section 15.5.4). Appearance and motion together. Motion models constrain where a target could go; learned appearance cues, such as embeddings from image crops or lidar shape descriptors, constrain who it likely is. A common composite cost is: C ij = λ m d 2 Mahalanobis (z j , Ëz i ) + λ a (1â cosSim(Ï j , Ï i )), where Ï j is a detection embedding and Ï i is a trackâs appearance model. Appear- ance reduces identity switches during occlusions and interactions, particularly in camera-heavy setups. Example 15.5.2 (Ambiguity at a pedestrian crossing). Two pedestrians cross paths inside each otherâs gates. GNN alone risks swapping identities, but JPDA soft- ens the update for both tracks across the ambiguous frames, while a modest appearance term, such as a color/texture embedding, stabilizes the assignment and preserves identities. 15.5.4 Track Management: Birth, Death, and Occlusion Bookkeeping is a critical function for tracking modules. Good track management balances eagerness to explain new detections with skepticism that avoids prolif- erating false tracks. 410 principles of robot autonomy Initiation. Common strategies to initialize tracks include: M/N logic, which con- firms a track only after M hits in the last N frames, and a score that accumu- lates matched updates and decays on misses. Initial covariances should be broad enough to reflect detector uncertainty and any depth ambiguity. Maintenance and missed detections. When a track receives no compatible detection, propagate its state through the motion model, inflate its covariance, and reduce its survival score. During short occlusions, this âcoastâ allows re-acquisition without identity breaks. Keep occluded tracks alive for at most T miss seconds or N miss frames, tunable to the scene. Termination and hygiene. Delete tracks that fail confirmation or whose survival score falls below threshold. Merge or split tracks when they overlap persistently or when one detection consistently explains two weak tracks better than the re- verse. Periodically purge stale hypotheses in MHT and stale appearance embed- dings. 15.5.5 Random Finite Set (RFS) Filters When the number of targets varies and clutter is heavy, it is natural to treat the set of objects as the fundamental random variable. RFS filters propagate distributions over sets rather than over a fixed list of tracks. PHD and GM-PHD. The Probability Hypothesis Density (PHD) filter evolves the first moment (intensity) of the target-set distribution, whose peaks correspond to likely targets. With linear-Gaussian models, the Gaussian-mixture PHD (GM- PHD) filter maintains a set of weighted Gaussians with birth and survival terms. This approach is efficient, handles births and deaths gracefully, and works well when targets are numerous but relatively weak, though it does not manage iden- tity explicitly. Labeled multi-Bernoulli and ÎŽ-GLMB. For identity-aware tracking, labeled multi- Bernoulli (LMB) and ÎŽ-GLMB filters maintain labeled tracks with existence prob- abilities. They offer principled handling of associations, births, and deaths within a Bayesian set framework. The cost is increased computation and bookkeeping, but in return they handle combinatorial association in a coherent manner and provide clean uncertainty accounting in heavy clutter. RFS methods are not always necessary, but they are useful when scenes are dense, clutter rates are high, and identity is secondary to coverage (PHD) or when a full Bayesian treatment of multi-target tracking with identities (ÎŽ-GLMB/LMB) is desired. sensor fusion and object tracking 411 15.5.6 Tracking-by-Detection and Modern Practice Modern trackers often follow a tracking-by-detection paradigm, where a perception module (classical or learned) produces frame-wise detections with uncertainties, a motion model predicts track states, and an association layer links detections to tracks. In camera-centric systems, learned appearance embeddings significantly improve identity stability. In lidar/radar-centric systems, motion and geometry dominate, and appearance plays a smaller role. Two themes recur: 1. Uncertainty matters. Downstream gating and data association work much better when the detector exports calibrated confidence and geometric covariance (or a proxy such as a covariance in BEV cells). Calibrate these heads before fusion, for example with temperature scaling for classification and reliability diagrams for regression. 2. Robustness to missing modalities. In adverse weather or partial failures, radar or thermal imaging may carry the burden. Design the tracker to operate with a subset of sensors by inflating the covariance of the missing modality, adjusting initiation thresholds, and tuning T miss accordingly. 15.5.7 Evaluation: Metrics and What They Mean Tracking quality is multi-faceted: we care about detection quality, geometric ac- curacy, and identity preservation. Three widely used metrics capture different aspects: MOTA/MOTP. Multiple Object Tracking Accuracy (MOTA) aggregates missed detections (FN), false positives (FP), and identity switches (IDSW) against the number of ground-truth objects (GT): MOTA = 1â FN + FP + IDSW GT . Multiple Object Tracking Precision (MOTP) summarizes localization error for cor- rectly matched pairs. MOTA is simple and interpretable, but it can obscure trade- offs between detection and identity. IDF1. The IDF1 metric measures the F1 score of correctly identified detections over all matches, focusing on identity preservation. It penalizes identity swaps more explicitly than MOTA and is useful when appearance cues play a central role. HOTA. Higher Order Tracking Accuracy (HOTA) balances localization, detec- tion, and association in a unified measure by scoring matched pairs over a range of thresholds and combining the resulting detection and association accuracies. It 412 principles of robot autonomy correlates better with human judgment in crowded scenes where identity stability matters. A mature evaluation typically reports at least one association-aware metric (IDF1 or HOTA) alongside MOTA/MOTP and includes qualitative sequences that reveal behavior under occlusion, crossing, and sensor degradation. For safety- critical systems, also monitor calibration: plot innovation/NIS statistics for associ- ated pairs to ensure the tracker remains consistent across conditions. Takeaway. A reliable tracker is not a single algorithm but a disciplined combi- nation of realistic motion models, measurements with honest uncertainties, ellip- soidal gating to filter clutter, association that respects ambiguity (using appear- ance where it helps), and careful track management. In dense or high-clutter regimes, RFS methods provide a principled alternative that scales gracefully. With these pieces in place, we return to the broader fusion story in the learning eraâfeature-level fusion in BEV, cooperative perception, and uncertainty calibra- tionâwhich shape where we fuse in modern stacks. 15.6 Learning-Era Fusion: Features, BEV, and Cooperation The probabilistic view from earlier sections remains the backbone of modern perception, but practice has shifted in two important ways. First, many systems now fuse features rather than raw measurements, often in a shared birdâs-eye view (BEV) representation that aligns modalities in space and time. Second, the locus of fusion has moved beyond a single box: vehicles and infrastructure exchange information, and filters increasingly sit downstream of learned modules whose outputs carry (or should carry) uncertainty. This section turns these trends into concrete design patterns. 15.6.1 Where to Fuse: Early, Mid, or Late? Classical fusion, as in Section 15.2.1, stacked raw measurements and operated directly on likelihoods. This remains the gold standard when bandwidth is ample and calibration is impeccable. Modern stacks, however, often benefit from two additional levels that better reflect computational and communication realities. Early fusion (raw space). Here we project measurements into a common geometric frame and combine them there. For example, we may project lidar points colored by camera radiance, radar rangeâDoppler points registered in the ego frame, and stereo depth âliftedâ into 3D. The reward is geometric precision and simple physics-based likelihoods. The cost is high bandwidth, strict time alignment, and sensitivity to small calibration errors: a milliradian of extrinsic yaw drift that is barely noticeable in an image can become a meter of error at long range when projected onto the ground. sensor fusion and object tracking 413 Mid-level fusion (feature space). In mid-level fusion, each sensor produces features via a learned backbone, and those features are then fused in a representation de- signed for downstream tasks. BEV has become the workhorse: by lifting multiple camera views, lidar, and radar into a ground-aligned grid, we obtain a scene de- scription that is geometry-aware, compact, and well matched to detection, track- ing, and planning. BEV removes ego-motion, naturally accommodates occupancy and flow estimates, and plays well with both filters and learned modules. Late fusion (decision space). At the other end of the spectrum we fuse decisions: detections, tracks, or occupancy tiles with associated uncertainties. Late fusion is bandwidth-efficient and maps neatly onto decentralized and distributed ar- chitectures (Section 15.4). The trade-off is reduced flexibility: once an upstream detector has committed to boxes or tracks, there are fewer opportunities to correct miscalibration or recover missed evidence. No single level is universally best. A pragmatic recipe is: 1. Centralize within a platform at mid-level (BEV), where most accuracy-per-byte gains lie. 2. Export late-level artifacts for cooperation across platforms. 3. Maintain an early-level path only where the safety case demands it 26 . 26 For example, an emergency braking stack that reads raw radar. 15.6.2 BEV and Transformer-Style Fusion The BEV idea is conceptually simple but rich in practice. We represent the local scene as a grid aligned with the ground plane and ego pose, and let each modality contribute to that grid in the way most natural for it. The payoff is a shared canvas on which geometry, semantics, and motion can be reasoned about jointly. From cameras to BEV. Multi-view image features f c extracted by a backbone are lifted into BEV either through explicit geometry, such as projecting along esti- mated depth, or attention mechanisms that aggregate image features at positions consistent with BEV queries. In the geometric case, a pixel (u, v) with depth Ë d back-projects to the camera frame and then to the ego frame: x ego = ego T cam Ë d K â1 ïŁź ïŁŻ ïŁ° u v 1 ïŁč ïŁș ïŁ» ,bin(x ego )7â BEV cell. In the attention-based case, a BEV query at ground point g gathers evidence across cameras with weights that depend on viewing geometry and learned com- patibility, sidestepping brittle monocular depth estimates. Lidar and radar to BEV. Lidar contributes 3D points whose heights and intensities can be pooled (min/max/mean, or learned pooling) into BEV features. Radar 414 principles of robot autonomy adds rangeâDopplerâangle evidence highlighting long-range velocities and all- weather robustness. After alignment, the result is a multi-modal BEV tensor B â R HĂWĂC in which nearby cells carry detailed geometry and far cells carry coarser, velocity-centered context. Temporal fusion. Scenes evolve and sensors report at different times. BEV benefits from temporal memory in two complementary forms: âą Geometric memory: warp the previous BEV by the ego motion between frames and aggregate it with the current BEV. This acts as a skip connection that respects kinematics. âą Learned memory: apply spatiotemporal attention over a short buffer of BEV frames so the model can ârememberâ moving actors. Both require ego motion used for alignment to be time-consistent with feature timestamps (Section 15.3.2). Otherwise, the network ends up compensating for misalignment rather than modeling the scene. Outputs and uncertainty. Heads attached to the multi-modal BEV tensor B pre- dict detections, occupancy, flow, and other downstream quantities. To keep fusion principled, these heads should emit calibrated confidences and, when possible, ge- ometric covariances or credible intervals for positions and extents (Section 15.6.6). These outputs feed directly into gating and association (Sections 15.5.2 and 15.5.3) and determine how heavily a filter should trust each piece of evidence. 15.6.3 From Features to Filters: The Adaptor Pattern Learned modules speak in logits, heatmaps, and BEV tensors; filters expect pseudo- measurements with covariances. An adaptor translates between these languages so that the Bayesian machinery from earlier sections can operate on learned out- puts without any sleight of hand. At its simplest, an adaptor takes a detection ( Ëp,ÎŁ net , s)âa position, a covari- ance proxy, and a confidenceâand turns it into a measurement z with covariance R suitable for a KF/EKF update. Algorithm 15.3 shows a simple per-detection adaptor. The crucial step is calibration: mappingÎŁ net to a covariance R whose empirical NIS statistics match Ï 2 quantiles on held-out data. Once calibrated, learned detections and classical sensors inhabit the same probabilistic currency. Example 15.6.1. A BEV detector yields a 3D position estimate Ëp and a covariance proxyÎŁ net from a covariance head. We define h(x) = p(x) and update a platform- centric track with z = Ëp and R = calib(ÎŁ net ), where calib is learned or fitted offline so that the NIS aligns with Ï 2 on validation sequences. The update then proceeds identically to a classical sensor, including gating and fault handling. sensor fusion and object tracking 415 Algorithm 15.3: Featureâ filter adaptor (per detection). Data: Network output ( Ëp,ÎŁ net , s) with position Ëp, covariance proxyÎŁ net , confidence s, track prior (ÎŒ â ,ÎŁ â ), calibration map calib(·), minimum confidence s min . Result: Updated track and logged NIS for calibration monitoring. if s < s min then Discard detection and return. zâ Ëp Râ calib(ÎŁ net ) Compute innovation Ìz and NIS. if NISâ€ Ï 2 d,α then Accept detection and update track with KF/EKF. else Down-weight or reject detection. 15.6.4 Cooperative Perception (V2X) A single vehicleâs field of view is limited by its own geometry and occluders such as other vehicles and buildings. By contrast, an intersection camera may âsee around corners,â and neighboring vehicles can reveal what an ego vehicle cannot. Cooperative perception asks three questions: what should we send, when should we send it, and how should the receiver fuse it while maintaining honest uncertainty? What to send. The tiers from Section 15.4.4 apply directly. Raw or ROI snippets are rich but expensive, mid-level BEV tiles carry useful context at manageable bitrate, and late-level tracks and occupancy are lightweight and easy to fuse. In practice, a layered strategy often works best: share BEV features for critical regions, such as blind corners, and share tracks elsewhere. When and how. Messages must carry measurement timestamps so receivers can place them correctly in their own timelines (Section 15.3.2). Relative pose be- tween agents should be maintained by a small filter on SE(3) with an associated covariance. That pose uncertainty should then propagate either into feature align- ment (wider attention kernels, conservative warps) or into track fusion weights. Fusing as if poses were perfect is a quick way to become overconfident. Fusion modes. Feature-level cooperation aligns and aggregates BEV tilesâoften with a learned attention block that explicitly accounts for pose uncertainty. Track- level cooperation uses the decentralized and distributed methods of Section 15.4; when independence is doubtful, Covariance Intersection (Section 15.2.2) provides a conservative backstop that preserves consistency. Example 15.6.2 (Occlusion busting at a four-way stop). A vehicle approaches a 416 principles of robot autonomy four-way stop occluded by a truck. The roadside unit (RSU) shares a narrow strip of BEV features covering the blind zone at 10 Hz. The vehicle fuses these with its own BEV via attention, then initializes two pedestrian tracks with cali- brated covariance. A neighboring vehicleâs track messages arrive a moment later; CI fuses them conservatively with the ego tracks, shrinking uncertainty without overstating confidence. The planner receives a consistent occupancy map and honest covariances. 15.6.5 Differentiable Filtering and Hybrid Models Filters and networks are complementary rather than competing tools. Three hy- brid patterns recur in modern systems. Learned dynamics residuals. We retain a physically grounded motion model (CV/CT) but allow a network to predict a residual acceleration or steering term from con- text (maps, intents, social cues). The filter then predicts with the combination of physics and learned residual, and the residualâs variance reflects confidence in the learned component. This improves short-term forecasting without abandon- ing structure. Learned measurement models. Instead of hand-coding the measurement model h(·), we can learn a mapping from features to pseudo-measurements with a co- variance head. The adaptor in Section 15.6.3 ensures that outputs are calibrated before they reach the filter, keeping NIS/NEES in check. Back-propagating through filters. For end-to-end tuning, we can unroll a few fil- ter steps and back-propagate through the Kalman updates to adjust the feeding network. However, some safeguards are important: keep gains and covariances positive definite, regularize to avoid collapsing uncertainty, and validate with held-out NIS/NEES so the filter remains a filter rather than a brittle function approximator. 15.6.6 Uncertainty You Can Trust Neural modules are powerful, but their confidence estimates are often miscali- brated, and fusion depends on honest uncertainty to avoid gates admitting out- liers and associations overcommitting. Three classes of tools help keep confidence in line with reality: calibration, deep ensembles (or evidential models), and con- formal prediction. Calibration (post-hoc). For classification, temperature scaling rescales logits z by a scalar T, chosen on a validation set, before applying softmax, improving calibra- tion 27 . For regression, we can fit an affine map from raw variance proxies to 27 C. Guo et al. âOn calibration of mod- ern neural networksâ. In: International conference on machine learning.PMLR. 2017, p. 1321â1330 sensor fusion and object tracking 417 empirical errors so predicted variances match residuals. Expected calibration er- ror (ECE) and reliability diagrams provide simple diagnostics and can be tracked across conditions. Deep ensembles and evidential models. Small ensembles 28 average predictions and 28 For example, three to five seeds. expose epistemic uncertainty via disagreement 29 . Evidential models predict pa- 29 B. Lakshminarayanan, A. Pritzel, and C. Blundell.âSimple and scalable predictive uncertainty estimation using deep ensemblesâ. In: Advances in neural information processing systems 30 (2017) rameters of a distribution over distributions 30 , so uncertainty grows in regions 30 Forexample,aNormalâInverse- Gamma for scalar regression. with limited training data 31 . Either way, we feed the resulting variance through 31 A. Amini et al. âDeep evidential re- gressionâ.In: Advances in neural in- formation processing systems 33 (2020), p. 14927â14937 the adaptor so that gates and gains respond quantitatively. Conformal prediction for finite-sample guarantees. Conformal methods wrap any base predictor and produce prediction sets with coverage 1â α without distribu- tional assumptions 32 . For detections, a simple nonconformity score is negative 32 A. N. Angelopoulos, S. Bates, et al. âConformal prediction: A gentle intro- ductionâ. In: Foundations and trendsÂź in machine learning 16.4 (2023), p. 494â591 log-likelihood or 1â IoU with ground truth on a calibration set. Given a (1â α) quantile q 1âα of scores, at test time we: âą accept only predictions with score†q 1âα , or âą inflate their covariance until the score would fall below q 1âα . This yields explicit, finite-sample control of false exclusion at the level of detec- tions flowing into the tracker and keeps downstream gating behavior predictable. 15.6.7 Asynchrony, Events, and Adverse Weather Two additional practical issues round out the learning-era picture. Event cameras and asynchrony. Event sensors report brightness changes at mi- crosecond latency with large dynamic range. They pair naturally with IMUs and frame cameras: the IMU stabilizes short-term motion, event sensors add blur-free edges, and frame cameras add texture. Fusion follows the same tim- ing discipline as in Section 15.3.2: respect timestamps, interpolate priors, and back-smooth when out-of-sequence events matter. All-weather complements. Rain, fog, and snow degrade cameras and lidar before radar and thermal imagers fail. Radar and thermal should therefore be treated as complementary modalities, not afterthoughts. During training, modality dropout in BEV fusion helps the network succeed when one input is missing. Online, the system inflates the covariance of degraded modalities and uses health scores (Section 15.3.6) to adapt sensor weights. The goal is graceful degradation: perfor- mance should bend under adverse conditions but not break. Takeaway. Learning-era fusion does not replace the Bayesian core; it builds on it. BEV provides a geometry-aware workspace where heterogeneous features meet, cooperative perception broadens the field of view, hybrid models let learning fill in what physics leaves out, and calibration and conformal wrappers keep uncertainty honest. 418 principles of robot autonomy 15.7 Summary Sensor fusion is not a single algorithm but a way of organizing information. We began with a probabilistic lens: sensors provide likelihoods over latent quanti- ties, and Bayesâ rule combines them into a posterior (Section 15.2). Under lin- earâGaussian assumptions this reduces to the Kalman filter with stacked multi- sensor updates and an additive information form; with mild nonlinearities, we can extend to EKF/UKF. Along the way we introduced practical methods: aug- menting states to absorb biases, gating with innovation tests to keep outliers from steering the estimate, andâwhen fusing external estimatesâpreferring conserva- tive schemes such as Covariance Intersection over fragile independence assump- tions. To implement fusion in practice, we saw the importance of respecting the tim- ing of measurements. Registration aligns frames so measurements are commen- surate; timing discipline ensures updates occur at the measurement timestamp, not arrival time; multi-rate and asynchronous updates are handled by âpredict- ing to the stamp, then stacking at the stampâ; and late packets are reconciled by fixed-lag smoothing (Section 15.3). These habits, though not glamorous, are what make filters behave in the real world as theory predicts. Fusion architectures then decide where fusion happens and what is exchanged. Centralized systems are statistically clean but bandwidth-hungry, while decen- tralized track-to-track fusion scales more easily provided independence holdsâor CI is used to maintain consistency when it does not. Distributed systems reach agreement by exchanging information over a graph, with consensus and CI- consensus as workhorses (Section 15.4). The same care with timestamps and covariances that serves a single robot becomes the glue that binds cooperating agents. We then explored object tracking, which reframes fusion as the estimation of entities that move, become occluded, and reappear. Practical trackers are built from modest parts: realistic motion models, measurements with honest uncer- tainty, ellipsoidal gating to reduce clutter, assignment algorithms that hedge ambiguity (GNN when scenes are clear, JPDA/MHT when they are not), and disciplined track management for births, deaths, and occlusions. In dense or high-clutter regimes, random-finite-set methods (PHD, LMB/ÎŽ-GLMB) offer a principled alternative (Section 15.5). Finally, we examined how modern practice shifts both the form of what we fuse and the locus of fusion. Mid-level feature fusion in BEV provides a geometry- aware canvas where cameras, lidar, and radar contribute according to their strengths. Cooperative perception extends that canvas across vehicles and infrastructure. Hybrid and differentiable designs let learned modules supply residual dynamics or measurement models while the filter keeps uncertainty honest. Calibration and conformal wrappers bridge learned outputs to the Bayesian core so that con- fidence is earned, not assumed (Section 15.6). If there is a single theme to carry forward, it is that uncertainty is as important sensor fusion and object tracking 419 a piece of information as state or control. When we model it, track it, and respect itâacross sensors, across time, and across machinesâfusion becomes not just a way to combine data but a method for building systems that remain useful when conditions are least friendly. To learn more. For a rigorous bridge from Bayesian filtering to multisensor fusion, Bar-Shalom, Li, and Kirubarajan [11] is a classic engineering text. Simon [137] provides a comprehensive treatment of KF/EKF/UKF, including numerical issues such as Joseph-form updates. Thrun, Burgard, and Fox [148] remains a gentle but thorough introduction to probabilistic robotics, Bayes filters, and thinking in likelihoods. Gustafsson [44] offers a compact, practice-oriented overview of statistical sensor fusion. For broad taxonomies and systems perspectives, the multisensor fusion handbook by Liggins I, Hall, and Llinas [83] is invaluable. For gating, assignment, and multi-target tracking, Blackman and Popoli [17] is the radar-informed standard that still underpins many engineered systems. Stone et al. [142] give a Bayesian perspective on multiple-target tracking and association that pairs well with JPDA/MHT. For the assignment layer itself, Kuhnâs Hungar- ian method 33 and Bertsekasâs auction algorithm 34 are canonical references. When 33 H. W. Kuhn. âThe Hungarian method for the assignment problemâ. In: Naval research logistics quarterly 2.1-2 (1955), p. 83â97 34 D. P. Bertsekas.âThe auction algo- rithm: A distributed relaxation method for the assignment problemâ. In: Annals of operations research 14.1 (1988), p. 105â 123 populations vary and clutter is heavy, Mahler [91] develops RFS theory from first principles. For practitioners, the Gaussian-mixture PHD filter by Vo and Ma [155] and the labeled RFS/ÎŽ-GLMB family by Vo, Vo, and Phung [156] provide concrete algorithms that scale to realistic scenes. The optimization view of fusion is well covered by Dellaert [27] for factor graphs and by Kaess et al. [63] for incremental smoothing and mapping. For inertial navigation specifically, Forster et al. [36] develop on-manifold preintegra- tion, which connects cleanly to bias-aware EKF and fixed-lag smoothing. For con- sensus and distributed information exchange over graphs, the tutorial by Olfati- Saber, Fax, and Murray [106] is the right starting point. When cross-covariances are unknown and independence is doubtful, Covariance Intersection 35 provides 35 S. J. Julier and J. K. Uhlmann. âA non- divergent estimation algorithm in the presence of unknown correlationsâ. In: Proceedings of the 1997 American Control Conference (Cat. No. 97CH36041). Vol. 4. IEEE. 1997, p. 2369â2373 a principled, conservative backstop. Calibration of neural confidences is essential before learned outputs are fused with classical sensors. Guo et al. [43] introduce temperature scaling for classifi- cation; deep ensembles 36 offer a robust baseline for epistemic uncertainty; and 36 B. Lakshminarayanan, A. Pritzel, and C. Blundell.âSimple and scalable predictive uncertainty estimation using deep ensemblesâ. In: Advances in neural information processing systems 30 (2017) evidential regression is a compact alternative for heteroscedastic regression 37 . 37 A. Amini et al. âDeep evidential re- gressionâ.In: Advances in neural in- formation processing systems 33 (2020), p. 14927â14937 For distribution-free, finite-sample coverage that plugs directly into gating, An- gelopoulos, Bates, et al. [5] provide a clear tutorial on conformal prediction. 15.8 Exercises The starter code for the exercises provided below is available online through GitHub. To get started, download the code by running in a terminal window: git clone https://github.com/StanfordASL/pora-exercises.git 420 principles of robot autonomy We denote Problems requiring hand-written solutions and coding in Python withand, respectively. Problem 1: Variance Reduction Consider the problem from Example 15.2.1 where we have two sensors that mea- sure the same quantity with Gaussian noise. In this exercise, derive the result from Example 15.2.1 that: ÎŒ = z 1 Ï 2 2 + z 2 Ï 2 1 Ï 2 1 + Ï 2 2 ,Ï 2 = Ï 2 1 Ï 2 2 Ï 2 1 + Ï 2 2 . Additionally, prove that from this result that Ï 2 < minÏ 2 1 , Ï 2 2 . Problem 2: Kalman Sensor Fusion In this problem, you will explore using the Kalman filter for sensor fusion for a simple 1D autonomous car. We will model the carâs motion using the kinematic model: Ì p = v, Ì v = a, Ì a = j, where p is the position, v is the velocity, and a is the acceleration, and the control is the jerk, j. Assuming we apply a constant jerk across each time step, we can discretize this model exactly with sampling time T as: p t+1 = p t + v t T + 1 2 a t T 2 + 1 6 jT 3 , v t+1 = v t + a t T + 1 2 jT 2 , a t+1 = a t + jT. We will consider three possible sensors: 1. An IMU that measures the acceleration, a, with zero-mean Gaussian noise with standard deviation Ï IMU . 2. A lidar sensor that measures the distance to a known object, which provides a measurement of the position p with zero-mean Gaussian noise with standard deviation Ï lidar . 3. A GNSS sensor that measures the position p, but is not operating correctly and has a constant bias error, b, such that z GNSS = p + b, and also zero-mean Gaussian noise with standard deviation Ï GNSS . In the notebook ch15/exercises/kalman _ sensor _ fusion.ipynb, complete the following exercises: 1. Implement the matrices A and B to define the system dynamics model: x t = Ax tâ1 + Bu t , sensor fusion and object tracking 421 based on the discrete time model above for the car. Then, implement the Kalman filter algorithm function kalman _ filter _ update. 2. For the sensor model: z t = Cx t + ÎŽ t ,ÎŽ t âŒN (0, R), implement the matrices C and R for each combination of sensor setups: (a) IMU only (b) IMU + lidar (c) IMU + lidar + GNSS How does the RMSE for the position estimate compare among these configu- rations? How does the GNSS sensorâs bias affect the estimate? For the IMU only configuration, should we expect this to be able to estimate the position well 38 ? 38 For a more in-depth study, take a look at the function observable defined in the utility library. This is a method from lin- ear control systems theory that can give us a theoretical analysis of if a sensor configuration will provide us with suf- ficient information. 3. Re-define the Kalman estimator matrices for an augmented system state that includes the GNSS sensor bias. Run the provided code to see how this affects the state estimate when using the GNSS sensor. 422 principles of robot autonomy References [4]A. Amini et al. âDeep evidential regressionâ. In: Advances in neural infor- mation processing systems 33 (2020), p. 14927â14937. [5]A. N. Angelopoulos, S. Bates, et al. âConformal prediction: A gentle in- troductionâ. In: Foundations and trendsÂź in machine learning 16.4 (2023), p. 494â591. [11]Y. Bar-Shalom, X. R. Li, and T. Kirubarajan. Estimation with applications to tracking and navigation: theory algorithms and software. John Wiley & Sons, 2001. [15]D. P. Bertsekas. âThe auction algorithm: A distributed relaxation method for the assignment problemâ. In: Annals of operations research 14.1 (1988), p. 105â123. [17]S. Blackman and R. Popoli. Design and Analysis of Modern Tracking Systems. Artech House, 1999. [27]F. Dellaert. âFactor graphs and GTSAM: A hands-on introductionâ. In: Georgia Institute of Technology, Tech. Rep 2.4 (2012). [36]C. Forster et al. âOn-manifold preintegration for real-time visualâinertial odometryâ. In: IEEE Transactions on Robotics 33.1 (2016), p. 1â21. [43]C. Guo et al. âOn calibration of modern neural networksâ. In: International conference on machine learning. PMLR. 2017, p. 1321â1330. [44]F. Gustafsson. Statistical Sensor Fusion. Studentlitteratur, 2013, p. 554. [59]S. J. Julier and J. K. Uhlmann. âA non-divergent estimation algorithm in the presence of unknown correlationsâ. In: Proceedings of the 1997 American Control Conference (Cat. No. 97CH36041). Vol. 4. IEEE. 1997, p. 2369â2373. [63]M. Kaess et al. âiSAM2: Incremental smoothing and mapping using the Bayes treeâ. In: The International Journal of Robotics Research 31.2 (2012), p. 216â235. [72]H. W. Kuhn. âThe Hungarian method for the assignment problemâ. In: Naval research logistics quarterly 2.1-2 (1955), p. 83â97. [74]B. Lakshminarayanan, A. Pritzel, and C. Blundell. âSimple and scalable predictive uncertainty estimation using deep ensemblesâ. In: Advances in neural information processing systems 30 (2017). [82]M. Liggins I, D. Hall, and J. Llinas. Handbook of multisensor data fusion: theory and practice. CRC press, 2017. [83]M. Liggins I, D. Hall, and J. Llinas. Handbook of multisensor data fusion: theory and practice. CRC press, 2017. [91]R. Mahler. Statistical multisource-multitarget information fusion. Artech, 2007. sensor fusion and object tracking 423 [106]R. Olfati-Saber, J. A. Fax, and R. M. Murray. âConsensus and Coopera- tion in Networked Multi-Agent Systemsâ. In: Proceedings of the IEEE 95.1 (2007), p. 215â233. [119]H. E. Rauch, F. Tung, and C. T. Striebel. âMaximum likelihood estimates of linear dynamic systemsâ. In: AIAA journal 3.8 (1965), p. 1445â1450. [137]D. Simon. Optimal State Estimation: Kalman, H â , and Nonlinear Approaches. John Wiley & Sons, 2006. [142]L. D. Stone et al. Bayesian multiple target tracking. Artech House, 2013. [148]S. Thrun, W. Burgard, and D. Fox. Probabilistic Robotics. MIT Press, 2005. [155]B.-N. Vo and W.-K. Ma. âThe Gaussian mixture probability hypothesis density filterâ. In: IEEE Transactions on signal processing 54.11 (2006), p. 4091â 4104. [156]B.-N. Vo, B.-T. Vo, and D. Phung. âLabeled random finite sets and the Bayes multi-target tracking filterâ. In: IEEE Transactions on Signal Process- ing 62.24 (2014), p. 6554â6567. [164]S. Zhang, Y. Bar-Shalom, and G. Watson. âTracking with multisensor out- of-sequence measurements with residual biasesâ. In: 2010 13th Interna- tional Conference on Information Fusion. IEEE. 2010, p. 1â8. Part IV Robot Decision Making This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 16 Finite State Machines The preceding parts of this book have endowed the robot with a comprehensive set of foundational competencies. In Part I, we explored how optimal control provides a powerful framework for generating and executing robot motion. Part I introduced the perceptual capabilities of the robot, examining the sensors and algorithms required to observe and interpret its environment. Part I endowed the robot with a representation of its own state within the world, covering the methods by which it estimates its pose and constructs maps of its surroundings. In this chapter, and Part IV as a whole, we move to a higher level of abstrac- tion: robot decision-making. Here, the focus of decision-making shifts from the fine-grained details of physical motion to the strategic choices a robot must make to achieve its long-term goals. To illustrate this distinction, consider a mission in which a robot must navigate from an initial location A to a pickup location B, retrieve a package, and deliver it to a destination C. Executing the motion from A to B relies on the planning and control techniques developed in Part I. Detect- ing and identifying the package requires the perceptual models of Part I, while successful navigation depends on the localization and mapping methods intro- duced in Part I. However, coordinating these capabilitiesâdeciding to navigate first, then grasp the object, then proceed to the delivery location, and to monitor and recover from failures along the wayârequires an additional layer of decision- making. This layer operates over a discrete set of task modes and action choices, complementing the robotâs continuous physical state. In this chapter, we introduce finite state machines as a foundational framework for modeling and implementing discrete decision-making in robots 1 . We begin by 1 L. Kaelbling et al. 6.01SC: Introduction to Electrical Engineering and Computer Sci- ence I. MIT OpenCourseWare. 2011 providing a mathematical definition of a finite state machine in Section 16.1, and then discuss some architecture options, computational challenges, and practical implementation approaches in Section 16.2. Finally, in Section 16.3, we discuss the main limitations of finite state machines that motivate more advanced decision- making frameworks, which we explore in Chapters 17-19. This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 428 principles of robot autonomy s 0 s 1 s 2 i 1 / o 1 i 0 / o 0 i 0 / o 1 i 2 / o 0 Figure 16.1: A graphical repre- sentation of a FSM with states S = s 0 , s 1 , s 2 , inputs I = i 0 , i 1 , i 2 and outputs O = o 0 , o 1 . The directed edges cor- respond to the next-state func- tions and the output associated with each edge is defined by the output function. For example, in this FSM, we show the tran- sition n(s 0 , i 1 ) ââ s 1 in the top left, along with the correspond- ing output o(s 0 , i 1 )ââ o 1 . 16.1 A Mathematical Model of Discrete Decision-Making Finite state machines (FSMs) are a computational modeling framework for sys- tems that can be in one of a finite number of discrete states at any given time. This framework is used in a wide variety of disciplines, including electrical en- gineering, linguistics, computer science, philosophy, biology, and more. We can use FSMs in several different ways, including to specify a desired program or behavior, to model and analyze a systemâs behavior, or to predict future behavior. Formally, we define a FSM by a finite set of states S, an input alphabet I, an output alphabet O, a nextâstate function n : SĂ I â S, and an initial state s 0 â S. Additionally, we can define an output function according to two standard conventions: âą Mealy machine: the output depends on the current state and the current input: o : SĂ I â O,o t = o(s t , i t ). âą Moore machine: the output depends only on the current state: Ì o : Sâ O,o t = Ì o(s t ). Graphically, we represent states S as nodes and admissible transitions as directed edges. For a Mealy machine, each edge is typically labeled with an input/output pair i/o, indicating that when input i is received in state s, the machine moves along that edge to s âČ = n(s, i) and produces output o = o(s, i). Equivalently, the output associated with a Mealy machine can be viewed as an annotation on each stateâinput pair (s, i). For a Moore machine, edges are labeled only by inputs 2 2 Since outputs do not depend on inputs. and each node is annotated with its output value. In what follows, we adopt the Mealy convention. Unless stated otherwise, we assume a deterministic 3 FSM with initial state s 0 , discrete time index t = 3 In a deterministic FSM, each state has only one transition for each possible in- put. 0, 1, 2, . . . , next state s t+1 = n(s t , i t ), and output o t = o(s t , i t ). This matches the graphical convention in Section 16.1, where nodes are states, directed edges encode input-driven transitions, and edge labels include both the triggering input and the resulting output. Example 16.1.1 (Parking Gate Control). Consider a parking gate control problem where the goal is to raise the gate when a car arrives, and then lower the gate finite state machines 429 Down Raising Lowering Up CAR _ DETECTED / RAISE GATE _ AT _ TOP / HOLD CAR _ CLEARED / LOWER GATE _ AT _ BOTTOM / HOLD TICK / RAISE TICK / LOWER TICK / HOLDTICK / HOLD Figure 16.2: FSM for the park- ing gate controller. Edges are la- beled input / output. when the car has passed. We assume sensors (or software events) indicate when a car is detected/cleared at the gate and when the gate reaches its end stops. The control actions are raising, lowering, or holding the gate position fixed. Note that in the real world, the position and velocity of the gate can vary continuously between the down and up positions. However, we use a higher-level discrete abstraction for the overall logic. We model the FSM in Mealy style with states: S : =Down, Raising, Up, Lowering, an input alphabet: I : =CAR _ DETECTED, CAR _ CLEARED, GATE _ AT _ TOP, GATE _ AT _ BOTTOM, TICK, and outputs: O : =RAISE, LOWER, HOLD. Here TICK is a periodic control-cycle event that lets the machine command con- tinuous motion between end-stop events. We define the next-state and output maps n(s, i) and o(s, i) with representative 430 principles of robot autonomy Figure 16.3: A graphical repre- sentation of the FSM for PX4. We can see that even for a rel- atively small number of states, the FSM is quite complex in or- der to model the full behavior of the system. Image retrieved from diydrones.com. rules: n(Down, CAR _ DETECTED) = Raising,o(Down, CAR _ DETECTED) = RAISE, n(Raising, GATE _ AT _ TOP) = Up,o(Raising, GATE _ AT _ TOP) = HOLD, n(Raising, TICK) = Raising,o(Raising, TICK) = RAISE, n(Up, CAR _ CLEARED) = Lowering,o(Up, CAR _ CLEARED) = LOWER, n(Lowering, GATE _ AT _ BOTTOM) = Down,o(Lowering, GATE _ AT _ BOTTOM) = HOLD, n(Lowering, TICK) = Lowering,o(Lowering, TICK) = LOWER, n(Down, TICK) = Down,o(Down, TICK) = HOLD, n(Up, TICK) = Up,o(Up, TICK) = HOLD. Example 16.1.1 shows the graphical representation of the FSM. 16.2 Finite State Machine Architectures One important practical disadvantage of FSMs is that their complexity does not scale well with system complexity, and, generally speaking, it can be time con- suming and challenging to design FSMs for practical robotic systems. To reduce complexity as much as possible, we must carefully choose the appropriate set of states to represent the system, and even with a well-defined set of states the in- teractions and transitions between states can be complex and hard to specify. For example, Section 16.2 shows a graphical representation of the FSM for the pop- ular open source flight software PX4 4 . Specifying the full behavior for a system 4 PX4 is a flight control software for drones and other unmanned vehicles. See https://px4.io/ for more informa- tion. like this can lead to a complex FSM, even if there are not very many states. At a high level, three complementary techniques help manage this complexity: âą State Minimization, which merges behaviorally equivalent states to remove re- dundancy. finite state machines 431 âą Hierarchical State Machines, which allow states to be nested within other states to create a hierarchy of states. âą Composition, where larger state machines are built from smaller, simpler state machines. 16.2.1 State Minimization A standard way to reduce an FSM without changing its inputâoutput behavior is partition refinement. Two states are equivalent if, for every input string, they produce the same output sequence (Mealy) or the same state outputs (Moore) and transition to equivalent states. Partition refinement starts from a coarse partitionâstates that are immediately distinguishable by their outputsâand re- peatedly refines the blocks by splitting states whose next states fall into different blocks for some input. When the process reaches a fixed point, merging the states within each block yields a minimal machine with the same behavior. We provide an example of this procedure in Example 16.2.1. Example 16.2.1 (Finite State Machine State Reduction). Consider a FSM that de- tects the input sequences 010 or 110. Example 16.2.1 lists the next-state function and the output function for each state and input. We can see that the states are the partial sequences and a Reset state, S : = 0, 1, 00, 01, 10, 11, Reset, the inputs are I : = 0, 1, and the outputs are the booleans O : = True, False that indicate if the sequence 010 or 110 has been created. For example, if the current partial sequence is 01 and a 0 is input, the next state will be the Reset state and the output will be True. State, sn(s, 0)n(s, 1)o(s, 0)o(s, 1) Reset01FalseFalse 00001FalseFalse 11011FalseFalse 00ResetResetFalseFalse 01ResetResetTrueFalse 10ResetResetFalseFalse 11ResetResetTrueFalse Table 16.1: FSM for a sequence detector that accepts digits 0 and 1 and outputs True if the se- quences 010 or 110 are gener- ated. We can now simplify this FSM by removing redundant states. To do so, we begin with the initial partition that groups states based on their output behavior: Reset, 0, 1, 00, 10 : always leads to a False output, 01, 11 : does not always lead to False output. We then further partition these sets based on the next-state function until we can- not make any further partitions. In the first step, we partition the setReset, 0, 1, 00, 10 into: Reset, 00, 10 : cannot transition to 01,11, 0, 1 : can transition to 01,11, 432 principles of robot autonomy and then partitionReset, 00, 10 into: Reset : can transition to0, 1, 00, 10 : cannot transition to0, 1. After applying the partition refinement procedure, the original seven states0, 1, 00, 01, 10, 11, Reset are reduced to four states, S new = 01, 11,0, 1,00, 10, Reset. The resulting machine, shown in Example 16.2.1, is therefore an equiva- lent 5 but reduced FSM. 5 Equivalent here meaning it has the same inputâoutput behavior. State, sn(s, 0)n(s, 1)o(s, 0)o(s, 1) Reset0,10,1FalseFalse 0,100,1001,11FalseFalse 00,10ResetResetFalseFalse 01,11ResetResetTrueFalse Table 16.2: Reduced FSM for the 010 or 110 sequence detector. 16.2.2 Hierarchical FSMs In some cases there are states that are not strictly equivalent but are closely re- lated in behavior. A common way to manage such structure is to use hierarchical finite state machines (HFSMs), also known as Statecharts 6 . HFSMs introduce super- 6 D. Harel. âStatecharts: A visual for- malism for complex systemsâ. In: Sci- ence of Computer Programming 8.3 (1987), p. 231â274 states 7 that group together related states into a higher-level state, and generalized 7 Also called composite states. transitions that allow transitions to and from these super-states. This reduces diagram clutter and mitigates state explosion by allowing behavior to be fac- tored and reused across related modes. Compared to flat FSMs, HFSMs support modularity, abstraction, and reuse of shared transitions at higher levels of the hierarchy. Harel [46] and Alur [3] provide an in-depth treatment of hierarchical FSMs, including formal definitions, semantics, and algorithms for analysis and verification. 16.2.3 Compositions We can also compose individual state machines in a variety of ways depending on their input/output behavior, including cascade compositions, parallel composi- tions, and feedback compositions. An example of each of these composition types is shown in Figure 16.4. Figure 16.4: Cascade, parallel, and feedback compositions of FSMs. Cascade (serial) composition. Let M 1 = (S 1 , I 1 , O 1 , n 1 , o 1 , s 1,0 ) and M 2 = (S 2 , I 2 , O 2 , n 2 , o 2 , s 2,0 ) be two FSMs and assume a wiring map Ï : O 1 â I 2 (often Ï is the identity after renaming). The cascade composition M = M 2 âŠ Ï M 1 is defined as finite state machines 433 the FSM M = (S, I, O, n, o, s 0 ) where: S = S 1 Ă S 2 , I = I 1 , O = O 2 , n((s 1 , s 2 ), i) = (n 1 (s 1 , i), n 2 (s 2 , Ï(o 1 (s 1 , i)))), o((s 1 , s 2 ), i) = o 2 (s 2 , Ï(o 1 (s 1 , i))), s 0 = (s 1,0 , s 2,0 ). (16.1) Intuitively, M 1 processes the external input i and produces an output o 1 (s 1 , i), which is then fed into M 2 as input via the wiring map Ï. Parallel (synchronous) composition. Parallel compositions combine two FSMs that share the same input alphabet and operate simultaneously on the same input. Let M 1 = (S 1 , I, O 1 , n 1 , o 1 , s 1,0 ) and M 2 = (S 2 , I, O 2 , n 2 , o 2 , s 2,0 ) be two FSMs with the same input alphabet I. The parallel composition M = M 1 â„ M 2 is defined as the FSM M = (S, I, O, n, o, s 0 ) where: S = S 1 Ă S 2 , I = I, O = O 1 Ă O 2 , n((s 1 , s 2 ), i) = (n 1 (s 1 , i), n 2 (s 2 , i)), o((s 1 , s 2 ), i) = (o 1 (s 1 , i), o 2 (s 2 , i)), s 0 = (s 1,0 , s 2,0 ). (16.2) Feedback composition. Feedback compositions connect (part of) an FSMâs output back to its input, creating a closed-loop system. The closed-loop machine is well- defined if the induced equations have a unique solution for the input given the output. Otherwise, the feedback composition is said to be ill-formed. 16.3 Limitations of Finite State Machines FSMs provide a clear, simple, and formally verifiable framework for discrete decision-making, which makes them attractive for implementing basic robot be- haviors. However, their effectiveness as the primary control architecture for com- plex autonomous robots is fundamentally limited. Although the architectural techniques discussed in Section 16.2 can alleviate some practical issues, they do not address the core limitations of the FSM paradigm. In practice, FSMs are best suited to highly structured environments in which the set of relevant situations and required responses is small, predictable, and can be exhaustively anticipated by a designer. As the complexity of a robotâs task and environment increases, FSM-based systems become difficult to scale. The number of states required to accurately 434 principles of robot autonomy represent the system can grow combinatorially with the number of factors that influence decision-making, including both the robotâs internal operating mode and aspects of the external world such as object configurations or the behavior of other agents. FSMs also tend to exhibit brittle behavior when deployed outside the situations explicitly anticipated by their designers. Because all transitions and responses must be hand-specified, the robot can only react meaningfully to inputs for which logic has been defined in advance. When confronted with novel objects, unmod- eled environmental changes, or unexpected sensor readings, the FSM lacks a mechanism for reasoning about new information or generalizing from prior ex- perience. As a result, reliable performance is difficult to achieve in open-ended or dynamic environments without extensive manual engineering. Another limitation of FSMs is the absence of an intrinsic notion of optimality. Standard FSMs describe which behaviors are permissible, but they do not pro- vide a formal way to evaluate or compare alternative actions in terms of cost, reward, or long-term objectives. While it is possible to encode heuristically cho- sen preferences through careful state and transition design, the FSM framework itself does not support principled decision-making based on the optimization of a defined performance criterion. The system simply executes the logic that has been predefined. Finally, the deterministic nature of FSMs makes them a poor match for the uncertainty inherent in real-world robotics. Sensor measurements are noisy, ac- tion outcomes are often stochastic, and the robotâs internal representation of the world is typically incomplete or approximate. Although designers can introduce states that qualitatively represent uncertainty, such as hypothesized or likely con- ditions, FSMs do not provide a principled mechanism for updating beliefs or making decisions based on probabilistic information. Taken together, these limitationsâpoor scalability, sensitivity to unanticipated situations, and the inability to reason explicitly about optimality and uncertaintyâ motivate the use of more expressive decision-making frameworks. In the next chapters, we will introduce decision-making frameworks which support opti- mization under stochastic dynamics, as well as extensions that address partial observability, and learning-based approaches that allow robots to acquire com- plex behaviors from data rather than relying solely on manual specification. 16.4 Summary In this chapter, we introduced finite state machines as a mathematical model for systems with discrete states and transitions. We defined the components of an FSM, including states, input and output alphabets, state transition functions, and output functions. We explored how FSMs can be represented using state dia- grams and transition tables, providing visual and tabular representations of their behavior. Recognizing that FSMs can rapidly grow in complexity, we discussed three architectural strategies for managing this complexity: state minimization, finite state machines 435 hierarchical finite state machines, and mechanisms for composing FSMs. To learn more. For a rigorous and comprehensive introduction to finite state ma- chines within the broader context of computation, a classic resource is the text- book by Sipser [139] on the theory of computation. The seminal paper by Harel [46] is essential reading for a deep understanding of hierarchical FSMs, which it introduced as Statecharts. For a more modern and formal textbook treatment of hybrid systems, including hierarchical and concurrent state machines, see Alur [3]. Finally, for the application of these concepts in the wider context of AI for robotics, we refer to the course by Kaelbling et al. [61]. 16.5 Exercises The starter code for the exercises provided below is available online through GitHub. To get started, download the code by running in a terminal window: git clone https://github.com/StanfordASL/pora-exercises.git We denote Problems requiring hand-written solutions and coding in Python withand, respectively. Problem 1: State Machine In this problem, you will create a finite state machine for a simple autonomous machine of your choice. For the system of your choice, define the set of states S, the input alphabet I, the output alphabet O, the nextâstate function n : SĂ I â S, and the initial state s 0 â S. Define the output function using the Mealy machine convention. Draw a diagram of the state machine similar to Example 16.1.1. 436 principles of robot autonomy References [3]R. Alur. Algorithms for Decision Making. MIT Press, 2015. [46]D. Harel. âStatecharts: A visual formalism for complex systemsâ. In: Sci- ence of Computer Programming 8.3 (1987), p. 231â274. [61]L. Kaelbling et al. 6.01SC: Introduction to Electrical Engineering and Computer Science I. MIT OpenCourseWare. 2011. [139]M. Sipser. Introduction to the Theory of Computation. International Thomson Publishing, 1996. 17 Sequential Decision Making and Dynamic Programming In Chapter 16, we introduced finite state machines as a structured and explicit way to model the logical flow of a robotâs behavior, allowing us to hand-design a set of rules that govern its actions in response to events. For well-defined tasks with a limited number of states, a carefully crafted finite state machine is an effec- tive and interpretable way to implement a robotâs decision-making logic. How- ever, the very structure that makes finite state machines clear also imposes fun- damental limitations, especially as the complexity and uncertainty of the robotâs environment grow. This chapter introduces a more general, optimization-based framework that directly addresses these limitations. Rather than manually specifying decision rules, we formulate decision-making as a mathematical optimization problem, allowing optimal actions to be computed automatically for complex, multi-step tasks, even under uncertainty. The central computational tool explored in this chapter is dynamic programming, a powerful algorithmic paradigm for solving sequential decision-making problems. Dynamic programming operates by de- composing complex, long-horizon problems into a sequence of simpler, nested subproblems that can be solved efficiently. As we will see, it also provides the theoretical foundation for modern learning-based approaches like reinforcement learning (Chapter 18) and imitation learning (Chapter 19). We begin in Section 17.1 by applying dynamic programming to deterministic decision-making problems, where the robotâs actions have certain and predictable outcomes. We then extend this framework in Section 17.2 to stochastic decision- making problems, specifically Markov decision processes (MDPs), which explic- itly account for uncertainty in the environment. Finally, in Section 17.3, we dis- cuss the limitations of classical dynamic programming methods, thereby motivat- ing the learning-based approaches introduced in later chapters. 17.1 Deterministic Sequential Decision Making We begin our study of dynamic programming by introducing a simple, yet ex- tremely general, formulation of sequential decision making. Despite its apparent This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 438 principles of robot autonomy simplicity, this formulation captures a wide range of problems arising in robotics, control, operations research, and artificial intelligence. We first consider the de- terministic case, where the outcome of each action is fully predictable, before addressing stochasticity in the next section. In this formulation, time is modeled as a sequence of discrete stages at which decisions are made. That is, we consider a discrete-time setting 1 , where the evo- 1 The continuous-time counterpart to dy- namic programming is the Hamilton- Jacobi-Bellman (HJB) equation.While the HJB equation is beyond the scope of this chapter, we refer interested read- ers to Bertsekas [12] for a comprehensive treatment of continuous-time decision- making. lution of the system is described by a difference equation of the form: x t+1 = f t (x t , u t ),t = 0, . . . , Tâ 1,(17.1) where x t â R n denotes the system state at time step t, u t â R m is the control applied at that step, and f t specifies how the state evolves. The integer T defines a finite planning horizon. At each time step, not all controls may be available. We therefore associate with each state x t a set of admissible controls, denoted byU(x t ), and impose the constraint: u t âU(x t ),t = 0, . . . , Tâ 1.(17.2) No particular structure is assumed for U(x t ). Depending on the application, it may be a finite set of discrete actions, a continuous region of allowable inputs, or a state-dependent subset encoding physical, logical, or resource limitations. The objective of the decision making problem is specified through an additive cost function defined over the planning horizon: J(x 0 , u 0 , . . . , u Tâ1 ) = g T (x T ) + Tâ1 â t=0 g t (x t , u t ),(17.3) where g t represents the stage cost incurred at time t and g T is a terminal cost applied to the final state. The additivity of the cost over time is a central structural assumption: it is this property that enables the decomposition of the problem into simpler subproblems, which lies at the heart of dynamic programming. No assumptions are made regarding smoothness, convexity, or time invariance of the cost functions. The deterministic sequential decision making problem can be formally defined as follows: Definition 17.1.1 (Deterministic Sequential Decision Making Problem). Given the discrete-time system (17.1), the control constraints (17.2), and the additive cost function (17.3), the deterministic sequential decision making problem can be stated as the following optimization problem: J â (x 0 ) = minimize u t , t=0,...,Tâ1 g T (x T ) + Tâ1 â t=0 g t (x t , u t ), subject to x t+1 = f t (x t , u t ),t = 0, . . . , Tâ 1, u t âU(x t ),t = 0, . . . , Tâ 1. (17.4) sequential decision making and dynamic programming 439 As we will see throughout the remainder of this chapter, the central goal of dynamic programming is to compute a solution to Problem (17.4) in the form of an optimal closed-loop control policy: Ï =Ï 0 , . . . , Ï Tâ1 , u â t =Ï â t (x t ),t = 0, . . . , Tâ 1, (17.5) where the policy Ï denotes a sequence of functions Ï t mapping any state x t into a control u t 2 . 2 In the deterministic setting considered here, the system evolution is fully pre- dictable, and closed-loop and open- loop optimal solutions therefore coin- cide. The true advantages of closed-loop policies emerge mainly in the stochas- tic setting, which we address in the next section. In Chapter 3, we were able to derive closed-loop control laws primarily by ex- ploiting strong structural assumptions, such as linear system dynamics, quadratic cost functions, or special system properties like differential flatness. While these assumptions yield elegant and computationally efficient solutions, they substan- tially limit the range of problems that can be addressed. Dynamic program- ming offers a fundamentally different approach. Rather than relying on restric- tive structural properties, it exploits only the sequential nature of the decision- making process and the additive structure of the cost function, thereby provid- ing a systematic framework for computing optimal closed-loop policies for a much broader class of systems, including those with nonlinear dynamics and non-quadratic costs. In what follows, we will explore how dynamic programming can be employed to solve the deterministic decision making problem defined above through the so-called principle of optimality. 17.1.1 The Principle of Optimality The dynamic programming approach to sequential decision making rests on a very simple, yet powerful, idea, known as the principle of optimality 3 . Despite 3 Often referred to as Bellmanâs principle of optimality. its simplicity, this principle is the key that transforms an otherwise intractable optimization problem into one that can be solved efficiently through recursive decomposition. At a high level, the principle of optimality expresses the following insight. Suppose that a policy is optimal for a given decision making problem. Then, if we consider any intermediate time and the state reached at that time while following this policy, the remaining decisions prescribed by the policy must themselves be optimal for the subproblem that starts from that state. In other words, any tail segment of an optimal policy must itself be optimal for the corresponding tail subproblem. The intuitive justification for this principle is straightforward. If the remaining decisions after some time were not optimal for the corresponding tail subproblem, then we could replace them with a better alternative and thereby reduce the total cost. This would contradict the assumption that the original policy was optimal. Thus, optimal policies must be composed of optimal solutions to all of their tail subproblems. This idea is most easily visualized in shortest-path problems, as illustrated in 440 principles of robot autonomy Section 17.1.1. If the optimal path from a starting point a to a destination e passes through an intermediate point b, then the portion of the path from b to e must itself be optimal among all paths that start at b. Otherwise, a shorter path from b to e could be substituted, yielding a shorter overall path from a to e. Figure 17.1: Illustration of the principle of optimality for a shortest-path problem.If the path a â b â e is optimal from a to e, then the subpath b â e must be optimal when starting from b. Adapted from Kirk [70, Ch. 3]. We now state the principle of optimality formally for deterministic decision making problems. Theorem 17.1.2 (Principle of Optimality (Deterministic Case)). Letu â 0 , u â 1 , . . . , u â Tâ1 be an optimal control sequence to the deterministic decision making problem defined by Problem 17.4 with initial condition x â 0 , and letx â 0 , x â 1 , . . . , x â T denote the corresponding optimal state trajectory. Then, for any time t â 0, . . . , Tâ 1, the truncated control sequenceu â t , . . . , u â Tâ1 is optimal for the subproblem that starts from state x â t at time t and minimizes the tail cost: J tail (x t , u t , . . . , u Tâ1 ) = g T (x T ) + Tâ1 â i=t g i (x i , u i ), subject to the same system dynamics and control constraints, over the horizon t to T. The principle of optimality suggests that an optimal policy can be constructed by solving a sequence of smaller subproblems. One may first solve the tail sub- problem involving only the final stage, then extend this solution to the tail sub- problem involving the last two stages, and continue in this manner until an op- timal policy for the entire horizon is obtained. The dynamic programming algo- rithm is based precisely on this idea: it proceeds backward in time, solving tail subproblems of increasing length by reusing solutions to shorter tail subprob- lems. Example 17.1.1 (Shortest-Path Problem). Consider the deterministic shortest-path problem shown in Example 17.1.1, where the objective is to find an optimal path from point b to point f . Suppose that the optimal costs from points c, d, and e to f are already known. A brute-force approach would require evaluating all possible paths from b to f , including: bâ câ f , bâ câ dâ f , bâ câ dâ eâ f , bâ dâ câ f , bâ dâ f , bâ dâ eâ f , bâ eâ dâ câ f , bâ eâ dâ f , bâ eâ f. sequential decision making and dynamic programming 441 Figure 17.2: By leveraging op- timal tail costs, the number of candidate paths that must be evaluated when searching from b to f is dramatically reduced. Adapted from Kirk [70, Ch. 3]. By exploiting the principle of optimality, we know that any optimal path from b to f must consist of an immediate step from b to one of its neighboring points (c, d, or e), followed by an optimal path from that neighboring point to f . Thus, only three candidate paths need to be evaluated: bâ câ f ,bâ dâ f ,bâ eâ f , where the optimal path can be found by selecting the minimum-cost path among: J bc + J â c f , J bd + J â d f , J be + J â e f . This simple example illustrates the central idea underlying dynamic program- ming, where optimal tail costs are reused to efficiently construct optimal solu- tions to larger problems. We now formalize this idea into a systematic algorithm for solving sequential decision making problems. 17.1.2 The Dynamic Programming Algorithm The principle of optimality provides a powerful structural property of optimal solutions, but by itself it does not specify how such solutions should be computed. Dynamic programming turns this structural insight into a concrete computational procedure to find optimal control policies. Rather than attempting to find an optimal control sequence over the entire horizon at once, dynamic programming proceeds by solving a sequence of smaller subproblems backward in time. This motivates the introduction of the so-called cost-to-go function. For each time t â 0, . . . , T and each state x t , define the op- timal cost-to-go J â t (x t ) as the minimum achievable cost when the system starts from state x t at time t and evolves optimally until the terminal time T. By defini- tion, the cost-to-go at time T coincides with the terminal cost: J â T (x T ) = g T (x T ), âx T âX , since no further decisions remain to be made after time T. The principle of optimality implies that the optimal cost-to-go functions satisfy a recursive relationship, where the cost-to-go at time t can be expressed in terms 442 principles of robot autonomy of the stage cost at time t and the cost-to-go at time t + 1: J â t (x t ) = min u t âU(x t ) h g t (x t , u t ) + J â t+1 f t (x t , u t ) i ,t = 0, . . . , Tâ 1.(17.6) This equation, often referred to as the Bellman equation, expresses the global optimization problem in terms of a local minimization combined with the optimal solution of a shorter-horizon problem. In practice, the dynamic programming algorithm leverages the Bellman equa- tion to compute the optimal cost-to-go functions via a backward-in-time recur- sion, as summarized in Algorithm 17.1. Starting from the known terminal cost J â T , one computes J â Tâ1 , then J â Tâ2 , and so on, until J â 0 is obtained. At each stage of this backward recursion, the optimal tail cost is computed for every state in the state space. The result is a collection of functions J â t (·) T t=0 that completely characterize the optimal performance of the system from any state and time. Algorithm 17.1: Dynamic Programming (Deterministic) J â T (x T ) = g T (x T ), for all x T âX for t = Tâ 1 to 0 do J â t (x t ) = min u t âU(x t ) h g t (x t , u t ) + J â t+1 f t (x t , u t ) i ,for all x t âX return J â 0 (·), . . . , J â T (·) Once the cost-to-go functions have been computed, the optimal control at each time step is obtained by minimizing the sum of the immediate cost and the opti- mal cost-to-go of the resulting next state: u â t = arg min u t âU(x â t ) h g t (x â t , u t ) + J â t+1 f t (x â t , u t ) i . The system is then propagated to the next state x â t+1 = f t (x â t , u â t ), and the process is repeated until the terminal time T is reached. Conceptually, the backward pass computes the optimal cost-to-go functions of every tail subproblem, while the forward pass uses these functions to compute the actions that realize these optimal costs. Although dynamic programming yields an exact solution to the deterministic decision making problem, its direct application is often limited by computational considerations. The backward recursion requires evaluating the Bellman equation for every possible state at every time step, which may be infeasible when the state space is continuous or very large. Discretization of the state space may render the algorithm implementable, but even then the computational burden can grow rapidly with the dimension of the state. These challenges motivate the development of approximate methods that retain the conceptual framework of dynamic programming while relaxing its computational demands. Despite these practical limitations, dynamic programming occupies a central role in sequential decision making. It provides the canonical solution method sequential decision making and dynamic programming 443 for finite-horizon problems, offers a precise interpretation of optimality through cost-to-go functions, and serves as the conceptual foundation for a wide range of learning-based control algorithms. Example 17.1.2 (Grid Navigation). Consider the environment shown in Exam- ple 17.1.2, where the objective is to move from point a to point h while incurring the minimum possible cost. The state x = a, b, c, d, e, f , g, h corresponds to the agentâs current location on the grid, and the available control actions at each state are encoded by the arrows indicating allowable directions of travel. For example, at point c the agent may move either right or up, but not left or down. Each directed edge is associated with a nonnegative traversal cost, as shown in the figure. Figure 17.3: A deterministic de- cision making problem where the goal is to move from point a to point h while incurring the minimal amount of cost.The path a â d â e â f â g â h is the optimal path. We solve this problem by dynamic programming in Example 17.1.2. Adapted from Kirk [70, Ch. 3]. We treat point h as a terminal state with zero terminal cost, so that: J â T (h) = 0, and we allow the agent to remain at h at zero cost once it is reached. The dy- namic programming recursion is initialized at this terminal condition and pro- ceeds backward in time, successively computing the optimal cost-to-go for states that can reach h within an increasing number of steps. At the first backward step, corresponding to one step from the terminal time, only states that can transition directly to h are relevant. These are the points e and g, together with h itself. The optimal costs-to-go are obtained by adding the immediate transition cost to the terminal cost: J â Tâ1 (h) = 0 + J â T (h) = 0,u â Tâ1 (h) = stay, J â Tâ1 (e) = 8 + J â T (h) = 8,u â Tâ1 (e) = right, J â Tâ1 (g) = 2 + J â T (h) = 2,u â Tâ1 (g) = up. At the next step of the recursion, states such as d and f become relevant, since they can reach h in two steps. Their optimal costs-to-go are computed by adding 444 principles of robot autonomy the immediate cost of moving to e or g and then using the previously computed one-step tail costs: J â Tâ2 (d) = 3 + J â Tâ1 (e) = 11,u â Tâ2 (d) = right, J â Tâ2 ( f ) = 3 + J â Tâ1 (g) = 5,u â Tâ2 ( f ) = right. At this point, the cost-to-go values represent the optimal cost of reaching h in at most two steps from each of the statesd, e, f , g. As the recursion continues, additional states enter the computation, and some states acquire multiple feasible paths to the terminal state. For example, at the next backward step, state e may either move directly to h or move downward to f and then proceed optimally from there. The Bellman equation automatically selects the cheaper of these alternatives: J â Tâ3 (e) = min8 + J â Tâ2 (h), 2 + J â Tâ2 ( f ) = 7,u â Tâ3 (e) = down. Similarly, the other relevant states at this step are updated as follows: J â Tâ3 (g) = 2,u â Tâ3 (g) = up, J â Tâ3 (d) = 3 + J â Tâ2 (e) = 11,u â Tâ3 (d) = right, J â Tâ3 ( f ) = 5,u â Tâ3 ( f ) = right, J â Tâ3 (a) = 8 + J â Tâ2 (d) = 19,u â Tâ3 (a) = right, J â Tâ3 (c) = min5 + J â Tâ2 (d), 3 + J â Tâ2 ( f ) = 8,u â Tâ3 (c) = right. At this stage, we see that the goal h is reachable from a in three time steps on path aâ dâ eâ h, and that we would incur a cost of 19. Extending the horizon further allows the algorithm to discover lower-cost paths that take advantage of additional intermediate states. In particular, the recursion continues one more step to yield: J â Tâ4 (e) = 7,u â Tâ4 (e) = down, J â Tâ4 (g) = 2,u â Tâ4 (g) = up, J â Tâ4 (d) = 3 + J â Tâ3 (e) = 10,u â Tâ4 (d) = right, J â Tâ4 ( f ) = 5,u â Tâ4 ( f ) = right, J â Tâ4 (a) = 8 + J â Tâ3 (d) = 19,u â Tâ4 (a) = right, J â Tâ4 (c) = min5 + J â Tâ3 (d), 3 + J â Tâ3 ( f ) = 8,u â Tâ4 (c) = right, J â Tâ4 (b) = 9 + J â Tâ3 (c) = 17,u â Tâ4 (b) = right. Finally, extending the horizon one last time allows the algorithm to find the op- timal path from the initial state a to the goal state h, corresponding to the path sequential decision making and dynamic programming 445 aâ dâ eâ f â gâ h, with total cost 18: J â Tâ5 (e) = 7,u â Tâ5 (e) = down, J â Tâ5 (g) = 2,u â Tâ5 (g) = up, J â Tâ5 (d) = 10,u â Tâ5 (d) = right, J â Tâ5 ( f ) = 5,u â Tâ5 ( f ) = right, J â Tâ5 (a) = min8 + J â Tâ4 (d), 5 + J â Tâ4 (b) = 18,u â Tâ5 (a) = right, J â Tâ5 (c) = min5 + J â Tâ4 (d), 3 + J â Tâ4 ( f ) = 8,u â Tâ5 (c) = right, J â Tâ5 (b) = 9 + J â Tâ4 (c) = 17,u â Tâ5 (b) = right. Several important features of dynamic programming are illustrated by this exam- ple. First, the algorithm does not search over complete paths from a to h. Instead, it incrementally builds optimal solutions by reusing previously computed tail costs. Second, the algorithm computes optimal costs and controls for all states, not just the initial state of interest. As a result, once the cost-to-go functions have been computed, optimal paths can be generated immediately from any starting point and for any horizon length. For example, starting from point c with a hori- zon of three steps, the optimal path c â f â g â h and its associated cost of 8 can be read off directly, without any additional computation. 17.2 Decision Making Under Uncertainty: Markov Decision Processes The deterministic decision making framework developed in Section 17.1 provides a clean and powerful lens through which sequential decision making problems can be understood and solved. In realistic robotic systems, however, the environ- ment is never perfectly known, and uncertainty is an intrinsic feature of physical interaction and perception. Sensor measurements are noisy, actuation is imper- fect, and the environment may evolve in ways that cannot be modeled exactly or anticipated in advance. As a result, the evolution of the system state cannot be described deterministically, and decision quality must be evaluated in a statistical sense. The goal of this section is to extend the deterministic sequential decision mak- ing problem in Definition 17.1.1 to explicitly account for such uncertainty. We be- gin by deriving a stochastic formulation of the sequential decision making prob- lem that incorporates uncertainty into both the system dynamics and the cost structure, thereby introducing the framework of Markov decision processes (MDPs). This framework is a cornerstone of modern decision making under uncertainty, and will serve as a bridge to the learning-based methods developed in Chapter 18 and Chapter 19. We then adapt the principle of optimality and the dynamic pro- gramming algorithm to this stochastic setting. 446 principles of robot autonomy 17.2.1 Problem Formulation We begin by modifying the state transition model in (17.1) to include a stochastic disturbance: x t+1 = f t (x t , u t , w t ),t = 0, . . . , Tâ 1,(17.7) where w t denotes a stochastic disturbance at time t. The disturbance w t is as- sumed to be drawn from a known conditional probability distribution: w t ⌠p t (w t | x t , u t ).(17.8) This assumption implies that the distribution of the next state depends only on the current state and action, and not on the full history of the system. This con- ditional independence assumption is an instance of the Markov property 4 , which 4 Which we previously encountered in Chapter 11 in the context of Bayesian fil- tering. states that, given the present state and action, the future evolution of the sys- tem is independent of the past. Accordingly, the state x t can be interpreted as a sufficient summary of all past information relevant for future decision making. The admissible control constraints remain unchanged. At each time step, the control must satisfy: u t âU(x t ),t = 0, . . . , Tâ 1,(17.9) where U(x t ) may encode physical limitations, logical constraints, or discrete ac- tion choices. We also allow the instantaneous cost to depend explicitly on the disturbance: g t :X ĂU ĂW â R,t = 0, . . . , Tâ 1,(17.10) whereW denotes the space of possible disturbance values. Because the system evolution is now stochastic, performance can no longer be evaluated along a single trajectory. Instead, we measure performance in expec- tation. Specifically, given a policy Ï = Ï 0 , . . . , Ï Tâ1 , we define the associated expected cost starting from an initial state x 0 as: J Ï (x 0 ) = E w " g T (x T ) + Tâ1 â t=0 g t (x t , Ï t (x t ), w t ) # ,(17.11) where the expectation is taken with respect to the joint distribution of the dis- turbance sequence w 0 , . . . , w Tâ1 induced by the policy Ï and the stochastic dynamics (17.7). This formulation corresponds to a risk-neutral objective, where policies are compared based on their average performance 5 . 5 While the expected cost formulation is the most common in the litera- ture, alternative risk measuresâsuch as worst-case performance or risk-sensitive criteriaâcan also be considered, but are beyond the scope of this chapter. We can now formally state the stochastic sequential decision making problem. Definition 17.2.1 (Stochastic Sequential Decision Making Problem). Given the stochastic dynamics (17.7), the control constraints (17.9), and the expected cost (17.11), the stochastic sequential decision making problem consists of computing an optimal policy: Ï â =Ï â 0 , . . . , Ï â Tâ1 , sequential decision making and dynamic programming 447 that solves: J â (x 0 ) = minimize Ï E w " g T (x T ) + Tâ1 â t=0 g t (x t , Ï t (x t ), w t ) # , subject to x t+1 = f t (x t , u t , w t ),t = 0, . . . , Tâ 1, u t = Ï t (x t )âU(x t ), t = 0, . . . , Tâ 1. (17.12) This formulation is also known as a finite-horizon Markov decision process. It mirrors the deterministic problem in structure, differing only in the introduction of random disturbances and the use of expected cost as the performance criterion. As we will see next, this similarity allows us to extend the principle of optimality and dynamic programming methods to this stochastic setting 6 . 6 This problem formulation lies at the core of several disciplines, including optimal control, operations research, robotics, economics, and machine learn- ing.As a result, it is common to encounter substantially different nota- tion and terminology across communi- ties, even when describing essentially the same underlying problem. In this chapter, we adopt notation that is stan- dard in optimal control.In the next chapter, we will reintroduce the same problem using notation that is more common in the machine learning and re- inforcement learning literature.For a broader discussion of the connections, overlaps, and distinctions among these perspectives, we refer the reader to Pow- ell [109]. 17.2.2 Stochastic Decision Making with Dynamic Programming As in the deterministic case, the key property that enables efficient solution meth- ods for the stochastic sequential decision making problem is the principle of opti- mality. In this subsection, we first state the principle of optimality for the stochas- tic setting, and then show how it leads to a dynamic programming recursion for computing optimal policies. Principle of Optimality. In the stochastic setting, the core intuition behind the principle of optimalityâthat optimal policies can be constructed by composing optimal solutions to tail subproblemsâremains valid. However, it must be for- mulated in terms of expected future cost. Because state transitions are random, it is no longer meaningful to reason in terms of a single âoptimal trajectoryâ. In- stead, policies are evaluated by the expected cumulative cost they induce under the stochastic elements in the system. The crucial assumption that enables a recursive decomposition is the Markov property. Specifically, as introduced in Chapter 11, the Markov property ensures that the future evolution of the system depends only on the current state and action, and not on the full history leading up to that state. Consequently, the expected cost incurred from time t onward depends only on x t and the future actions selected by the policy. Under this assumption, if a policy is optimal from the initial condition, then after reaching any intermediate state x t , the remaining portion of that policy must still be optimal for the tail problem that starts at x t . If this were not the case, the tail could be replaced by an alternative policy with strictly lower expected cost, thereby reducing the overall expected cost and contradicting the optimality of the original policy. Formally, we can state the principle of optimality for the stochastic decision making problem as follows: Theorem 17.2.2 (Principle of Optimality (Stochastic Case)). Let Ï â =Ï â 0 , Ï â 1 , . . . , Ï â Tâ1 be an optimal policy for the stochastic decision making problem defined in Problem 17.12. 448 principles of robot autonomy For any time t and any state x t that is reachable under Ï â , the tail policyÏ â t , . . . , Ï â Tâ1 is an optimal policy for the tail subproblem that starts at time t from state x t and mini- mizes the expected cost: J Ï (x t ) = E w " g T (x T ) + Tâ1 â i=t g i (x i , Ï i (x i ), w i ) # . As in the deterministic case, this result implies that optimal policies can be constructed by solving a sequence of nested tail subproblems, with dynamic pro- gramming providing a systematic procedure for carrying out this backward con- struction, as we describe next. Dynamic Programming. The dynamic programming algorithm for the stochastic case closely mirrors its deterministic counterpart. It proceeds backward in time, starting from the terminal cost: J â T (x T ) = g T (x T ), âx T âX , and successively computing the optimal cost-to-go for earlier stages. At each step, the algorithm evaluates, for every state, the expected cost associated with each admissible control and selects the minimizing one. Similarly, the cost-to-go can be expressed recursively, yielding the (stochastic) Bellman equation: J â t (x t ) = min u t âU(x t ) E w g t (x t , u t , w t ) + J â t+1 f t (x t , u t , w t ) ,t = 0, . . . , Tâ 1. (17.13) The resulting dynamic programming algorithm is summarized in Algorithm 17.2. Algorithm 17.2: Dynamic Programming (Stochastic Case) J â T (x) = g T (x), for all xâX for t = Tâ 1 to 0 do J â t (x) = min uâU(x) E w g t (x, u, w) + J â t+1 ( f t (x, u, w)) , for all xâX return J â 0 (·), . . . , J â T (·) Once the cost-to-go functions have been computed, an optimal policy is ob- tained by selecting: Ï â t (x t ) = arg min u t âU(x t ) E w g t (x t , u t , w t ) + J â t+1 f t (x t , u t , w t ) .(17.14) Example 17.2.1 (Inventory Control). Consider a simple inventory control problem in which the state x t â N denotes the available stock of an item at time t. At each step, the decision maker chooses how many items to order, u t â N, before facing a random demand w t â N. The system dynamics are given by: x t+1 = max0, x t + u t â w t , sequential decision making and dynamic programming 449 which captures the fact that demand reduces inventory, restocking increases it, and inventory cannot go below zero. We impose the constraint: x t + u t †2, so that the inventory capacity is limited to at most two units. Demand is modeled as a discrete random variable with the following proba- bility distribution: p(w t = 0) = 0.1,p(w t = 1) = 0.7,p(w t = 2) = 0.2. We consider a finite horizon of T = 3 steps and define the expected cost: E w " 2 â t=0 u t + (x t + u t â w t ) 2 # , which penalizes both ordering costs and the squared deviation between inventory and demand. To solve this problem using dynamic programming, we first identify the state and control spaces: X =0, 1, 2, U(x t ) =0, 1, 2â x t . Following Algorithm 17.2, we initialize the terminal cost-to-go function at time t = 3 as: J â 3 (x 3 ) = 0, âx 3 â0, 1, 2, since no costs are incurred after the final stage. Working backward, we compute the cost-to-go at time t = 2 by minimizing the expected one-step cost: J â 2 (0) = minimize u 2 â0,1,2 E w h u 2 + (u 2 â w 2 ) 2 i , = minimize u 2 â0,1,2 u 2 + 0.1u 2 2 + 0.7(u 2 â 1) 2 + 0.2(u 2 â 2) 2 = 1.3, J â 2 (1) = minimize u 2 â0,1 E w h u 2 + (1 + u 2 â w 2 ) 2 i , = minimize u 2 â0,1 u 2 + 0.1(1 + u 2 ) 2 + 0.7(u 2 ) 2 + 0.2(u 2 â 1) 2 = 0.3, J â 2 (2) = E w h (2â w 2 ) 2 i = 0.1(2) 2 + 0.7(1) 2 + 0.2(0) 2 = 1.1. Note that for x 2 = 2, the only admissible action is u 2 = 0 due to the inventory capacity constraint, and hence the minimum is achieved at u 2 = 0. From these computations, we directly obtain the optimal actions at time t = 2: Ï â 2 (0) = 1, Ï â 2 (1) = 0, Ï â 2 (2) = 0. 450 principles of robot autonomy Continuing this process, we compute the cost-to-go at time t = 1: J â 1 (0) = minimize u 1 â0,1,2 E w h u 1 + (u 1 â w 1 ) 2 + J â 2 (max0, u 1 â w 1 ) i = 2.5, J â 1 (1) = minimize u 1 â0,1 E w h u 1 + (1 + u 1 â w 1 ) 2 + J â 2 (max0, 1 + u 1 â w 1 ) i = 1.5, J â 1 (2) = E w h (2â w 1 ) 2 + J â 2 (max0, 2â w 1 ) i = 1.68, with optimal stage actions: Ï â 1 (0) = 1, Ï â 1 (1) = 0, Ï â 1 (2) = 0. Finally, in the last step: J â 0 (0) = minimize u 0 â0,1,2 E w h u 0 + (u 0 â w 0 ) 2 + J â 1 (max0, u 0 â w 0 ) i = 3.7, J â 0 (1) = minimize u 0 â0,1 E w h u 0 + (1 + u 0 â w 0 ) 2 + J â 1 (max0, 1 + u 0 â w 0 ) i = 2.7, J â 0 (2) = E w h (2â w 0 ) 2 + J â 1 (max0, 2â w 0 ) i = 2.818, with: Ï â 0 (0) = 1, Ï â 0 (1) = 0, Ï â 0 (2) = 0. For this example, the optimal policy is time-invariant: order one unit when the inventory is empty, and order nothing otherwise. This policy balances the risk of unmet demand against the cost of carrying inventory, and it emerges naturally from the dynamic programming recursion. 17.3 Limitations of Dynamic Programming Dynamic programming is a powerful algorithmic framework that underlies a wide range of methods for solving sequential decision-making problems. How- ever, despite its generality and conceptual elegance, it suffers from several impor- tant practical limitations. First, in its standard form, dynamic programming requires perfect knowledge of the environment. This includes an accurate model of the system dynamicsâ whether deterministic or stochasticâas well as a known cost or reward function. In many real-world applications, particularly in robotics, this requirement can be highly restrictive, as physical interactions involving friction, contact dynamics, or complex nonlinear effects are often difficult to model accurately. Moreover, the algorithms presented in this chapter assume that the full system state is known and directly observable, which is often not the case in practice. sequential decision making and dynamic programming 451 A second major limitation of dynamic programming is the so-called curse of di- mensionality. The computational and storage requirements of dynamic program- ming grow exponentially with the dimension of the state space. Concretely, if the state is n-dimensional and each state variable can take on M discrete values, then the Bellman equation must be evaluated M n times at each stage of the al- gorithm. While this may be tractable for low-dimensional problems, it quickly becomes infeasible as the dimensionality increases. This issue is especially rele- vant in robotics, where the state and action spaces can be very high-dimensional due to the presence of multiple degrees of freedom, sensors, and actuators. Together, these limitations motivate the development of alternative approaches that relax some of the assumptions underlying classical dynamic programming. In particular, they have led to the study of learning-based methods that trade exact optimality for computational tractability. In the next chapter, we first introduce reinforcement learning, which can be viewed as a natural extension of the ideas developed here. At a high level, rein- forcement learning may be viewed as a form of approximate dynamic programming, as it retains the central concepts of value functions, Bellman recursions, and pol- icy improvement, while addressing the key limitations of classical dynamic pro- gramming by allowing policies to be learned from interaction rather than com- puted from a perfectly known model over an explicitly enumerated state space. For this reason, reinforcement learning provides the most direct next step after the present chapter. We then turn to imitation learning, which offers a broader and highly prac- tical framework for robot learning from demonstrations. Rather than learning solely through trial-and-error interaction with the environment, imitation learn- ing leverages expert behavior to acquire control policies, and has become one of the most widely used paradigms for training robotic systems in practice. Taken together, reinforcement learning and imitation learning provide two complemen- tary responses to the limitations of classical dynamic programming, and they will be the focus of Chapter 18 and Chapter 19, respectively. 17.4 Summary In this chapter, we introduced a general, optimization-based framework for se- quential decision-making that allows us to compute optimal policies for complex tasks, even under uncertainty. We began in Section 17.1 by framing deterministic sequential decision-making as a discrete-time optimal control problem, introduc- ing dynamic programming as the primary computational tool for solving such problems. Grounded in the principle of optimality, dynamic programming al- lows long-horizon problems to be decomposed into a sequence of simpler, recur- sively defined subproblems. In Section 17.2, we extended this framework to the stochastic setting, where uncertainty in system dynamics and outcomes must be explicitly accounted for. This led to the formulation of Markov decision processes and the use of dynamic programming to compute policies that optimize expected 452 principles of robot autonomy performance under known sources of randomness. Finally, in Section 17.3, we discussed the practical limitations of dynamic programming, most notably the curse of dimensionality and the reliance on accurate models of the environment. These challenges motivate the development of learning-based approaches, which are the focus of the next chapter. To learn more. For a foundational and comprehensive treatment of dynamic pro- gramming and sequential decision-making, Dynamic Programming and Optimal Control by Bertsekas [12] is an indispensable resource, providing rigorous deriva- tions for both deterministic and stochastic problems. For an introduction to Markov decision processes with a focus on their role in modern artificial intelli- gence and robotics, Reinforcement Learning: An Introduction by Sutton and Barto [143] is the standard reference. A more in-depth mathematical treatment of MDPs can be found in Markov Decision Processes: Discrete Stochastic Dynamic Programming by Puterman [110]. 17.5 Exercises The starter code for the exercises provided below is available online through GitHub. To get started, download the code by running in a terminal window: git clone https://github.com/StanfordASL/pora-exercises.git We denote Problems requiring hand-written solutions and coding in Python withand, respectively. Problem 1: Shortest Path Through A Grid Consider the graph shown in Section 17.5, where it is only possible to move to the right and the numbers associated with each edge represent a cost to traverse that edge. The decision to be made at each node is whether to go âupâ or âdownâ, and we can assume the state transitions are deterministic. For this exercise: 1. Use dynamic programming to find the shortest path from A to B. 2. Consider a generalized version of the shortest path problem in Section 17.5 where the grid has n segments along each side. Find the number of compu- tations required by an exhaustive search algorithm (i.e., the number of routes that such an algorithm would need to evaluate) and the number of compu- tations required by a DP algorithm (i.e., the number of DP evaluations). For example, for the n = 3 case shown in Section 17.5, an exhaustive search algo- rithm requires 20 computations, while the DP algorithm requires only 15. sequential decision making and dynamic programming 453 AB 5 7 6 8 7 9 10 6 10 8 10 6 7 5 9 5 12 7 8 6 7 9 10 11 Figure 17.4:Simple grid for shortest-path problem. Problem 2: Machine Maintenance Suppose we have a machine that is either running or is broken down. If it runs throughout one week, it makes a gross profit of $100. If it fails during the week, gross profit is zero. If it is running at the start of the week and we perform preventive maintenance, the probability that it will fail during the week is 0.4. If we do not perform maintenance, the probability of failure is 0.7. However, preventative maintenance will cost $20. When the machine is broken down at the start of the week, it may either be repaired at a cost of $40, in which case it will fail during the week with a probability of 0.4, or it may be replaced at a cost of $150 by a new machine that is guaranteed to run through its first week of operation. Find the optimal repair, replacement, and maintenance policy that maximizes total profit over four weeks, assuming a new machine at the start of the first week (that is guaranteed to run during the first week of operation). Problem 3: Markovian Drone In this problem, we will model the task of flying a drone to its destination through a storm as a Markov Decision Process (MDP), and solve for the optimal policy using dynamic programming. The world is represented as an nĂ n grid, so the state space is: X : =(x 1 , x 2 )â R 2 | x 1 , x 2 â0, 1, . . . , nâ 1. In these coordinates, (0, 0) represents the bottom left corner of the map and (nâ 1, nâ 1) represents the top right corner of the map. From any location x = (x 1 , x 2 )âX , the drone has five possible controls it can apply: U : =up, down, left, right, land. 454 principles of robot autonomy The corresponding state changes for each control input are: âą up: (x 1 , x 2 )7â (x 1 , x 2 + 1) âą down: (x 1 , x 2 )7â (x 1 , x 2 â 1) âą left: (x 1 , x 2 )7â (x 1 â 1, x 2 ) âą right: (x 1 , x 2 )7â (x 1 + 1, x 2 ) âą land: (x 1 , x 2 )7â (x 1 , x 2 ) Additionally, there is a storm centered at x eye â X . The stormâs influence is strongest at its center and decays farther from the center according to the equation Ï(x) = exp â â„xâx eye â„ 2 2 2Ï 2 . Given its current state x and control input u, the droneâs next state is determined as follows: âą The control to land is deterministic. âą With probability Ï(x), the storm will cause the drone to move in a uniformly random direction (for non-landing controls). âą With probability 1â Ï(x), the drone will move in the direction specified by the control (for non-landing controls). âą If the resulting movement would cause the drone to leave X , then it will not move at all. For example, if the drone is on the right boundary of the map, then moving right will do nothing. The quadrotorâs objective is to reach x goal â X as quickly as possible, so the cost function is the indicator function g t (x t ) = 1â I x goal (x t ). In other words, the drone will receive a cost of 1 whenever it is not at x goal â X , and a cost of 0 at the goal. The drone has limited fuel capacity, so it must reach the goal in at most T timesteps. If the drone is not at the goal at the end of the horizon, it will crash and we will incur a cost of 100 to replace the drone, therefore g T (x T ) = 100(1â I x goal (x T )). In the notebook ch17/exercises/markovian _ drone.ipynb, complete the following exercises: 1. Implement the functions of DroneMDP to compute the optimal cost-to-go values via dynamic programming (Algorithm 17.2) and extract the optimal policy using Equation (17.14). 2. Given T = 100, n = 20, Ï = 10, x eye = (15, 15), and x goal = (19, 9), compute and plot a heatmap of the optimal cost-to-go for t = 0 over the grid X . Then, using the code provided, simulate the MDP with the optimal policy for T = 100 time steps with the state initialized at x = (0, 19). sequential decision making and dynamic programming 455 References [12]D. Bertsekas. Dynamic Programming and Optimal Control. Athena Scientific, 2000. [70]D. E. Kirk. Optimal Control Theory: An Introduction. Dover Publications, 2004. [109]W. B. Powell. âAI, OR and control theory: A Rosetta Stone for stochastic optimizationâ. In: Princeton University. 2012. [110]M. Puterman. Markov Decision Processes: Discrete Stochastic Dynamic Pro- gramming. Wiley, 2014. [143]R. Sutton and A. Barto. Reinforcement learning: An introduction. MIT Press, 2018. 18 Reinforcement Learning In Chapter 17, we introduced the deterministic and stochastic sequential decision- making problems, and showed how they can be solved using dynamic program- ming. These approaches, however, suffer from two major limitations: (i) they typically assume full knowledge of the system dynamics, and (i) their com- putational complexity grows exponentially with the dimensionality of the state space. In this chapter, we provide an introduction and overview of the field of Reinforcement Learning 1,2 (RL). At a high level, reinforcement learning can be de- 1 D. Bertsekas. Reinforcement learning and optimal control. Athena Scientific, 2019 2 R. Sutton and A. Barto. Reinforcement learning: An introduction.MIT Press, 2018 scribed as the problem of learning what to doâthat is, learning a mapping from states to control actionsâwith the goal of maximizing a cumulative numerical reward. More specifically, reinforcement learning formalizes learning through interaction. Rather than being explicitly instructed on the correct actions to take, an agent must discover effective strategies through trial and error, guided only by feedback from the environment. Crucially, unlike the methods discussed in the previous chapter, reinforcement learning does not require prior knowledge of the system dynamics or even full observability of the environment. This makes it a highly general and practical framework for autonomous decision-making in complex and uncertain environments. In this chapter, we begin in Section 18.1 by introducing key concepts and theo- retical foundations of the reinforcement learning problem. Next, in Section 18.2, we introduce reinforcement learning algorithms based on exact dynamic pro- gramming that leverage ideas from Chapter 17. Motivated by practical limita- tions of dynamic programming, we introduce two foundational model-free learn- ing paradigms known as Monte Carlo methods and temporal-difference learning in Section 18.3. Finally, after introducing a taxonomy of reinforcement learning algorithms in Section 18.4, we discuss widely used model-free and model-based algorithms in Section 18.5 and Section 18.6, respectively. 18.1 The Reinforcement Learning Problem At its core, reinforcement learning represents a mathematical formalism for learning- based decision making. While the mathematical formulationâwhich entails solv- ing an optimal control problem within an incompletely-known Markov decision This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 458 principles of robot autonomy process (MDP)âwill be detailed in the remainder of this chapter, the basic idea is simple: capture the essential aspects of a learning agent interacting over time with its environment to achieve a goal or objective. Within the landscape of machine learning paradigms, there are a number of key distinctions and specific challenges that are unique to reinforcement learning. Learning Without a Teacher. Supervised learning, arguably the most extensively studied paradigm in machine learning, assumes access to a dataset of labeled examples provided by a knowledgeable supervisor, with the objective of learning to imitate the supervisorâs behavior. While highly effective in many settings, this paradigm is ill-suited for interactive decision-making problems, where labeled data is unavailable or prohibitively expensive to obtain. Reinforcement learning, by contrast, is fundamentally concerned with learning from interaction, where the agent is not told which actions to take, but must instead learn through trial and error, guided solely by evaluative feedback from the environment in the form of rewards or penalties. This absence of an explicit teacher introduces a distinct set of challenges. Exploration vs. Exploitation. In reinforcement learning, the agent must resolve the intrinsic trade-off between exploration and exploitation. On the one hand, it must explore the environment to discover potentially rewarding actions; on the other hand, it must exploit its current knowledge to maximize cumulative reward. Focusing exclusively on exploration prevents the agent from capitalizing on what it has learned, while focusing solely on exploitation risks converging to a subop- timal policy. Effective learning therefore requires the agent to judiciously balance both behaviorsâtrying a diverse set of actions while progressively concentrating on those that yield the highest returns. This trade-off is largely absent in other learning paradigms and remains a defining challenge of reinforcement learning. Delayed Rewards. In many real-world problems, feedback from the environment is delayed or sparse. Rewards may only be observed after a sequence of actions has been executedâfor example, in chess, where the outcome is revealed only at the end of the game. Such delayed feedback introduces the challenge of credit assignment, making it difficult for the agent to determine which actions were responsible for the observed outcome. Data Not I.I.D. A core assumption underlying much of statistical learning theory is that data points are independent and identically distributed (i.i.d.). Reinforce- ment learning clearly violates this assumption, where the data collected by the agent is inherently sequential and highly correlated, since each action influences both the future states of the environment and the data observed thereafter. This dependence fundamentally changes both the theoretical analysis and practical design of learning algorithms. reinforcement learning 459 In the following sections, we introduce the mathematical formalism of rein- forcement learning and the key components that define the problem, together with strategies for addressing the challenges outlined above. 18.1.1 Elements of Reinforcement Learning In this and previous chapters, we have encountered and briefly discussed several key elements of a reinforcement learning system: a policy, an environment, a reward signal, a value function, and, optionally, a model of the environment. In this section, we provide a more formal definition of these elements and discuss how they interact in the context of the reinforcement learning problem. Policy. A policy, Ï(u | x), is a mapping from states to actions that defines the agentâs behavior. Depending on the application, the policy can take the form of a simple function, such as a lookup table or a parametric function, or a more complex one, for instance involving an explicit search process. In general, policies may be either deterministic, mapping each state to a single action, or stochastic, defining a probability distribution over actions. Environment. The environment is the system the agent interacts with. We math- ematically represent the environment by a transition model, p(x âČ | x, u), which defines the probability of transitioning to a new state, x âČ , given the current state, x, and action, u. The transition model may also be either deterministic or stochas- tic, depending on the nature of the environment. Reward signal. A reward signal defines the goal of the agent. At each time step, we assume the agent receives a scalar reward, r â R, from the environment that indicates how well the agent is performing. Reward signals are deterministic or stochastic functions of the state of the environment and the action taken, and we denote the function that produces the reward as R(x, u). Value function. While the reward signal represents an immediate measure of per- formance, the value function represents performance in the long run. Specifically, the value of a state defines how much reward the agent can expect to accumulate from that state onwards. This is clearly different from immediate reward. For example, a state might have a low immediate reward but a high value if it usu- ally leads to states with high rewards, and vice versa. An effective agent chooses actions by considering the value of the action rather than just the immediate re- ward. Because of this, numerous reinforcement learning algorithms are typically centered around accurately estimating values. Model. Lastly, a model of the environment is an optional component of the re- inforcement learning problem that represents the agentâs understanding of the environment. The modelâs goal is to mimic the behavior of the environment, and 460 principles of robot autonomy can be used to make hypotheses about how the environment will evolve 3 . In 3 For example, we can use models to evaluate different actions before execut- ing them. this chapter, we explore reinforcement learning algorithms that use models for learning, referred to as model-based algorithms, as well as more direct model-free algorithms that do not attempt to learn a model of the environment and solely focus on discovering optimal policies by trial-and-error learning. At a high level, most reinforcement learning algorithms follow the same ba- sic learning cycle. First, the agent interacts with the environment by observing the state x t , applying an action u t from a chosen behavior policy 4 , and then ob- 4 The behavior policy does not neces- sarily have to match the learned policy Ï(u| x). serving the next state x t+1 and scalar reward r t . This procedure, as illustrated in Section 18.1.1, may repeat for multiple steps, during which the agent uses the observed transitions (x t , u t , r t , x t+1 ) to update its policy. Agent Environment Control u t R(x t , u t ) x t+1 Reward r t State x t Figure 18.1: The reinforcement learning problem consists of an agent that learns how to make decisions by interacting with the environment. At each time step t, the agent observes the cur- rent state x t and selects an ac- tion u t to execute. The environ- ment then transitions to a new state x t+1 and provides a re- ward R(x t , u t ) to the agent. This process continues as the agent learns to optimize its actions based on the received rewards. 18.1.2 Problem Formulation Let us briefly revisit the MDP framework, which provides a standard mathemat- ical formalization of the reinforcement learning problem. Formally, an MDP is defined as a tuple: M = ( X ,U , p, R, Îł ) , whereX denotes the state space, which is the set of all possible environment states xâX and can be either discrete or continuous,U denotes the action space, which is the set of admissible actions u â U and can also be discrete or continuous, p characterizes the system dynamics through the transition probability distribution p(x t+1 | x t , u t ), R : X ĂU â R specifies the reward function, and Îł â (0, 1] is a discount factor that determines the relative importance of future rewards. From a reinforcement learning perspective, the goal is to learn a policy defined as a probability distribution over actions given states, Ï(u | x). We will use the term trajectory to refer to a sequence of states and actions of length T, given by: Ï : = (x 0 , u 0 , . . . , x T ), reinforcement learning 461 where T may be infinite. Given a policy Ï, the induced trajectory distribution p Ï is: p Ï (Ï) = p 0 (x 0 ) Tâ1 â t=0 Ï(u t | x t ) p(x t+1 | x t , u t ).(18.1) where p 0 is the initial-state distribution. The reinforcement learning objective is to maximize the expected discounted cumulative reward under this trajectory distribution, namely: V Ï T : = E Ï⌠p Ï (Ï) " Tâ1 â t=0 Îł t r t # .(18.2) An additional concept required to fully characterize V Ï T is that of discount- ing. In particular, the discount factor Îł is a scalar value in the range [0, 1] that determines the relative importance of future rewards, whereby a smaller Îł will make the agent focus more on immediate rewards, while a larger Îł will make the agent give more importance to future rewards. For example, in the limit case with Îł = 0, the agent will only consider immediate rewards, while in the case with Îł = 1, the agent will consider all future rewards equally. Mathematically, discounting is also crucial in ensuring that the sum of rewards in V Ï is finite even in the infinite horizon case with T =â, such that if Îł < 1 and the rewards r t are bounded, the sum of the rewards will be finite. In practice, the choice of Îł is often problem-dependent, and it is common to use a value close to 1 to ensure that the agent considers future rewards. 18.1.3 Value Functions and Bellman Equations Almost all reinforcement learning algorithms involve estimating value functions. At its core, a value function is a function of state or state-action pairs that defines how good 5 it is for the agent to be in a given state or to take a given action in a 5 As defined in the previous section, value functions quantify quality in terms of expected cumulative future rewards. given state. Since the reward an agent expects to receive in the future depends on the actions it will take, the value function is inherently defined with respect to a particular policy Ï. We define the state-value function, V Ï (x), as the expected sum of future rewards when starting in state x and following policy Ï thereafter 6 : 6 Throughout this chapter, we primarily consider the infinite-horizon case when referring to V Ï , although the same con- cepts extend to the finite-horizon case. V Ï (x) : = E Ï⌠p Ï (Ï) " â â k=0 Îł k R(x t+k , Ï(x t+k ))| x t = x # .(18.3) Similarly, the action-value function, Q Ï (x, u), is the expected return when start- ing in state x, taking action u, and then following policy Ï thereafter: Q Ï (x, u) : = E Ï⌠p Ï (Ï) " â â k=0 Îł k R(x t+k , Ï(x t+k ))| x t = x, Ï(x t ) = u # .(18.4) A key property of value functions used in the context of reinforcement learning and dynamic programming is that they satisfy the Bellman equations. The Bellman equations describe a recursive relationship that decomposes the value of a state 462 principles of robot autonomy or state-action pair into the immediate reward and the value of the next state or state-action pair. Formally, for any policy Ï and any state x, the Bellman equation defines the following self-consistency condition: V Ï (x) = E uâŒÏ(·|x) h R(x, u) + ÎłE x âČ âŒ p(·|x,u) V Ï (x âČ ) i ,(18.5) where, to simplify notation, we have omitted the time index t and used x âČ to denote the next state. Similarly, the Bellman equation for the action-value function is: Q Ï (x, u) = R(x, u) + ÎłE x âČ âŒ p(·|x,u), u âČ âŒÏ(·|x âČ ) Q Ï (x âČ , u âČ ) .(18.6) Crucially, the value functions V Ï and Q Ï are unique solutions to the Bellman equations. In the remainder of this chapter, we show how we can use the Bellman equa- tions to derive algorithms for estimating and approximating value functions, and how we can use these value functions to derive optimal policies. Central to the solution of reinforcement learning problems are the notions of optimal policies and optimal value functions. The term optimal derives from the fact that value functions induce a partial ordering over policies, where a policy Ï is said to be better than or equal to another policy Ï âČ if its value is no worse in every state. Formally, we write Ï â„ Ï âČ if and only if: V Ï (x)â„ V Ï âČ (x), âxâX . An optimal policy Ï â is a policy that is better than or equal to all other policies, such that Ï â â„ Ï for all policies Ï. While the optimal policy does not need to be unique, all optimal policies share the same optimal value function V â , defined as: V â (x) : = max Ï V Ï (x), âxâX . Similarly, optimal policies also share the same optimal action-value function Q â (x, u), defined as: Q â (x, u) : = max Ï Q Ï (x, u), âxâX , uâU . As discussed above, V â and Q â are value functions for the optimal policy, thus, they must satisfy the Bellman equations with respect to the optimal policy. How- ever, because V â and Q â are the optimal value functions, the Bellman equations can be written in a policy-independent form. This is achieved by exploiting the fact that, under an optimal policy, the value of a state is equal to the expected return obtained by selecting the best available action in that state. Formally, the Bellman equations for the optimal state-value function and action- value functionâreferred to as the Bellman optimality equationsâcan be derived by substituting the expectation over the policy from Equation (18.5) and Equa- tion (18.6) with a maximization over actions, and are given by: V â (x) = max u h R(x, u) + ÎłE x âČ âŒ p(·|x,u) V â (x âČ ) i ,(18.7) Q â (x, u) = R(x, u) + ÎłE x âČ âŒ p(·|x,u) max u âČ Q â (x âČ , u âČ ) .(18.8) reinforcement learning 463 Why are V â and Q â so central to reinforcement learning? The key reason is that, once either of these functions is known, deriving an optimal policy is substantially simpler. In particular, an optimal policy can be derived by acting greedily with respect to the optimal value functions. For example, given the optimal state-value function V â , the optimal policy can be computed as: Ï â (x) = arg max u h R(x, u) + ÎłE x âČ âŒ p(·|x,u) V â (x âČ ) i .(18.9) That is, once V â is known, determining the optimal policy reduces to a one-step lookahead where, at each state, the agent selects the action that leads to successor states with the highest expected value. Access to the optimal action-value function, Q â , simplifies the process even further. For any state x, we can obtain the optimal policy by selecting the action that maximizes Q â : Ï â (x) = arg max u Q â (x, u).(18.10) Thus, by representing a function over states (or state-action pairs), the optimal value functions allow for the direct computation of the optimal policy. 18.2 Dynamic Programming Methods As we introduced in Chapter 17, the key idea of dynamic programming is to de- compose a complex problem into simpler subproblems. This is achieved by using value functions to systematically organize and structure the search for optimal policies. In this section, we show how we can leverage dynamic programming algorithms in the context of reinforcement learning by turning the Bellman equa- tions into iterative update rules for the estimation of value functions. In particu- lar, we explore how to use dynamic programming ideas to derive algorithms for two distinct but interconnected tasks: prediction and control. Definition 18.2.1 (Prediction). In the context of reinforcement learning, we often refer to the task of estimating the value function for a given policy as prediction. Definition 18.2.2 (Control). In the context of reinforcement learning, we often refer to the task of finding the optimal policy as control. 18.2.1 Prediction: Policy Evaluation We first consider the prediction problem of estimating the value function, V Ï , un- der a given policy Ï. According to the Bellman equation in Equation (18.5), the value of a state x under policy Ï is defined as an expectation with respect to the policy and state transition model. For simplicity, we assume that the state transi- tion model and policy describe probability distributions over discrete states and actions, respectively, which allows us to express the expectations in the Bellman equation as sums rather than integrals 7 : 7 The extension to continuous states and actions is fundamentally equivalent and requires the replacement of summations with integrals. 464 principles of robot autonomy V Ï (x) = â uâU Ï(u| x) " R(x, u) + Îł â x âČ âX p(x âČ | x, u)V Ï (x âČ ) # .(18.11) Policy evaluation is an iterative algorithm to solve the prediction problem. Con- sider a sequence of approximations to the value function, denoted as V 0 , V 1 , V 2 , . . . , V Ï , where V 0 is an arbitrarily chosen initial guess 8 . Policy evaluation uses the Bell- 8 Under the condition that any terminal state, occurring when t = T in the finite- horizon setting or when the episode ter- minates in the infinite-horizon setting, must be assigned a value of zero. man equation in Equation (18.11) as an update rule, such that at iteration k, the value function for all states x âX is updated according to: V k+1 (x) = â uâU Ï(u| x) " R(x, u) + Îł â x âČ âX p(x âČ | x, u)V k (x âČ ) # .(18.12) It is important to note that V k = V Ï is a fixed point of the update rule in Equa- tion (18.12) since the Bellman equation for V Ï ensures equality in this case. More- over, under mild regularity conditions, it can be shown that the sequence of value functionsV k converges to V Ï as kââ. As we will see in the remainder of this chapter, the ideas described above are at the core of many reinforcement learning algorithms, including both model-based and model-free methods. 18.2.2 Policy Improvement Having introduced an approach for solving the prediction problem, namely, es- timating the value function under a fixed policy, we now turn to the problem of control, where the objective is to compute an optimal policy. To address the control problem, we rely on the policy improvement theorem, which provides a principled mechanism for transforming a given policy into a new policy that is guaranteed to be better than or equal to the original. Consider two policies, Ï and Ï âČ , such that for all states x âX : Q Ï (x, Ï âČ (x))â„ V Ï (x).(18.13) Then, the policy Ï âČ is guaranteed to be better than or equal to Ï, such that: V Ï âČ (x)â„ V Ï (x). Intuitively, if Ï âČ chooses actions that are at least as goodâaccording to the action- value function of Ï in Equation (18.13)âas those prescribed by Ï in every state (i.e., V Ï (x)), then following Ï âČ cannot result in worse long-term performance than continuing to follow Ï. Consider the greedy policy Ï âČ , which selects, in each state, the action that max- imizes the action-value function Q Ï associated with policy Ï 9 : 9 In other words, the greedy policy per- forms a one-step lookahead using the current state-value function V Ï . Ï âČ (x) : = arg max u Q Ï (x, u) = arg max u " R(x, u) + Îł â x âČ âX p(x âČ | x, u)V Ï (x âČ ) # . (18.14) By construction, this greedy policy satisfies the condition of the policy improve- ment theorem in Equation (18.13) and is therefore guaranteed to be better than or reinforcement learning 465 equal to the original policy. We refer to the process of constructing a new policy by greedily selecting actions with respect to the current value function as policy improvement. Suppose now that the greedy policy Ï âČ is as good as the original policy Ï, such that V Ï âČ = V Ï . From the definition of the greedy policy in Equation (18.14), we have: V Ï âČ (x) = max u " R(x, u) + Îł â x âČ âX p(x âČ | x, u)V Ï (x âČ ) # . Since V Ï âČ = V Ï , this expression is equivalent to the Bellman optimality equation in Equation (18.7). Therefore, the value function V Ï must equal the optimal value function, and the policy Ï âČ is therefore optimal. In other words, policy improvement provides a systematic procedure for it- eratively improving a policy using its value function, and it converges once the optimal policy is reached. 18.2.3 Control: Policy Iteration The policy improvement theorem provides a concrete strategy to improve a policy by greedily selecting actions with respect to the current value function. In this section, we discuss how we use this strategy, in tandem with policy evaluation, to construct an algorithm for finding the optimal policy. We refer to this algorithm for finding the optimal policy as policy iteration. At a high level, the key idea of policy iteration is as follows: starting from a given policy Ï, we first evaluate it to obtain its value function V Ï and then improve the policy using this value function to produce a new policy Ï âČ . The improved policy Ï âČ is subsequently evaluated to compute V Ï âČ , which is in turn used to derive an improved policy Ï âČ . This alternating process of policy evaluation and policy improvement is repeated until the policy converges to the optimal policy. More formally, policy iteration defines a sequence of monotonically improving policies by alternating between policy evaluation and policy improvement: V 0 PE ââ Ï 0 PI ââ V 1 PE ââ Ï 1 PI ââ V 2 PE ââ Ï 2 PI ââ . . . , where PE denotes policy evaluation and PI denotes policy improvement. We outline the policy iteration algorithm in Algorithm 18.1. Policy iteration is guaranteed to converge to the optimal policy and value func- tion, given enough iterations. In practice, since the policy evaluation step is an iterative algorithm, we typically initialize the value function to the value function from the previous step of policy iteration. This can increase the speed of conver- gence since the value function does not typically change substantially between iterations. Example 18.2.1 (Grid World Policy Iteration). Explore an implementation of pol- icy iteration for a simple grid-world environment in the repository github.com/StanfordASL/pora-exercises in the notebook 466 principles of robot autonomy Algorithm 18.1: Policy Iteration Data: Initial policy, Ï, and value function, V 0 , arbitrarily initialized for all xâX . Result: Policy, Ï â Ï â , and value function, V Ï â V â . for i = 0, . . . ,â do Policy Evaluation: for k = 0, . . . ,â do for x âX do V k+1 (x) = â uâU Ï(u| x) [ R(x, u) + Îł â x âČ âX p(x âČ | x, u)V k (x âČ ) ] ifâ„V k+1 â V k â„ < Δ then V Ï = V k+1 break Policy Improvement: for x âX do Ï âČ (x) = arg max u [ R(x, u) + Îł â x âČ âX p(x âČ | x, u)V Ï (x âČ ) ] if policy has converged then return Ï â Ï â and V Ï â V â Ï = Ï âČ ch18/policy _ iteration.ipynb. The grid-world environment is a simple grid with action space: U : =up, down, left, right, that has a set of absorbing states that get a reward of 0, and every other state gets a reward of â1. For this world, the state transitions are deterministic, so taking an action is guaranteed to result in moving in the desired direction (i.e. transition occurs with probability 1), but we will consider a stochastic policy, Ï(u | x). In the notebook for this example: 1. Run the provided code to see how the policy evaluation algorithm updates for a simple random policy. Try playing around with the discount factor to see how it affects the value function. 2. Take a look at the policy iteration algorithm code. Note that since we are considering a stochastic policy we use the softmax function: Ï âČ (u| x) = e ÎČQ Ï (x,u) â u âČ âU e ÎČQ Ï (x,u âČ ) , to define the policy probability, where ÎČ is the Boltzmann constant that when increased will decrease the entropy of the distribution for x (i.e. as ÎČââ the policy will approach being deterministic). Play around with the value of ÎČ to see how it affects the optimal value function. reinforcement learning 467 18.2.4 Control: Value Iteration One drawback of policy iteration is that it requires a full policy evaluation step at each iteration. As a result, the algorithm can be computationally expensive, since it must wait for the value function to converge before performing a policy improvement step, which only happens in the limit 10 . To address this limitation, 10 Several variants of policy iteration mit- igate this cost by using truncated or ap- proximate policy evaluation steps. value iteration provides an alternative approach that combines policy evaluation and policy improvement into a single step. Value iteration defines the following update rule: V k+1 (x) = max u " R(x, u) + Îł â x âČ âX p(x âČ | x, u)V k (x âČ ) # .(18.15) Starting from an arbitrary initial value function V 0 , the sequence of value func- tions V k generated by value iteration is guaranteed to converge to the optimal value function V â . Value iteration can be interpreted through the lens of the Bellman optimality equation, where the update in Equation (18.15) corresponds to applying the Bell- man optimality operator from Equation (18.7) to the current value function V k . In contrast to policy iteration, which alternates between iteratively solving the Bell- man equation for a fixed policy and performing a separate policy improvement step, value iteration directly applies the Bellman optimality equation at every iteration. We outline the complete value iteration algorithm in Algorithm 18.2. Algorithm 18.2: Value Iteration Data: Initial value function, V 0 , arbitrarily initialized for all xâX . Result: Policy, Ï â Ï â , and value function, V Ï â V â . for k = 0 toâ do for x âX do V k+1 (x) = max u [ R(x, u) + Îł â x âČ âX p(x âČ | x, u)V k (x âČ ) ] ifâ„V k+1 â V k â„ < Δ then V Ï = V k+1 Ï(x) = arg max u [ R(x, u) + Îł â x âČ âX p(x âČ | x, u)V k (x âČ ) ] , for all xâX return V Ï â V â and Ï â Ï â Policy iteration and value iteration are two of the most foundational algorithms in reinforcement learning and are the basis for many modern reinforcement learn- ing algorithms. 18.2.5 Inheriting the Limitations of Dynamic Programming Dynamic programming methods, including policy iteration and value iteration, are powerful tools for solving MDPs. However, these methods also inherit the limitations of dynamic programming as they require a complete model of the environment, p(x âČ | x, u), to compute the expectations in the Bellman equations, 468 principles of robot autonomy and are computationally expensive for large state and action spaces. In the next sections, we discuss two key approaches to address these limitations. We first introduce sampling methods, which relax the requirement of having a complete model of the environment, and then we introduce the concept of function approxi- mation, which helps address the computational complexity of dynamic program- ming methods. 18.3 Learning Paradigms for Model-free Control In this section, we introduce two classes of learning methods that estimate value functions and compute optimal policies without requiring a complete model of the environment: Monte Carlo (MC) methods and temporal-difference (TD) learning. These methods are particularly effective in practice since they are applicable to a wide range of problems where modeling the environment dynamics is either im- practical or infeasible. Similar to our discussion of dynamic programming meth- ods, we first address the prediction problem for both Monte Carlo and temporal- difference methods before extending the analysis to the control problem. 18.3.1 Monte Carlo Methods The term Monte Carlo broadly refers to a class of algorithms that rely on ran- dom sampling to estimate quantities of interest. In the context of reinforcement learning, Monte Carlo methods represent a class of approaches for solving the re- inforcement learning problem based on averaging observed cumulative rewards from experience 11 . We assume that the agent interacts with the environment for 11 We often use the terms samples or ex- perience to refer to sequences of states, actions, and rewards collected by inter- acting with the environment. a fixed number of time steps T, referred to as an episode, during which it collects a trajectory consisting of states, actions, and rewards. Episodic MDPs naturally describe tasks with well-defined beginnings and endings, such as navigating a maze, playing a game, or completing a robotic assembly. At the end of each episode, the agent uses these observed trajectories to estimate quantities of inter- est, such as value functions or policy updates. Monte Carlo methods for solving the prediction problem aim to learn the value function V Ï (or equivalently, Q Ï ) given a policy Ï. In this section, we first address the problem of using Monte Carlo methods for learning the state-value function V Ï , and then extend the discussion to learning the action-value function Q Ï . Since the value of a state is defined as the expected cumulative reward obtained when starting from that state, it can be estimated by averaging the cumulative rewards observed across episodes that visit the state. As the agent visits a state more frequently, the estimate of its value becomes increasingly accurate. Formally, suppose we wish to learn the value V Ï (x) of the state x under the policy Ï, using a set of N episodes Ï 1 , Ï 2 , . . . , Ï N passing through x. The state- value function can be estimated using the Monte Carlo prediction procedure out- lined in Algorithm 18.3. In particular, Monte Carlo prediction updates the value reinforcement learning 469 Algorithm 18.3: Monte Carlo Prediction Data: Initial value function, V, arbitrarily initialized for all xâX . Result: Value function estimate, V Ï . Initialize the state visit count, N(x) = 0, for all xâX for each episode Ï i =x 0 , u 0 , r 0 , x 1 , u 1 , r 1 , . . . , x T do for t = Tâ 1 to 0 do Compute the cumulative future reward from state x t : G t = â Tâ1 k=t Îł kât r k Increment the visit count: N(x t )â N(x t ) + 1 Update the value estimate: V(x t )â V(x t ) + 1 N(x t ) (G t â V(x t )) return V â V Ï estimate V(x t ) using the incremental update rule: V(x t )â V(x t ) + 1 N(x t ) (G t â V(x t )), which moves the current estimate toward the observed return G t , where N(x t ) denotes the number of times state x t has been visited. This incremental form provides an efficient way to compute sample averages and is equivalent to the empirical mean: V(x t ) = 1 N(x t ) N(x t ) â i=1 G i t , where G i t is the cumulative future reward observed in the i-th visit to state x t . We can also use Monte Carlo methods for learning action values 12 . At a high 12 Learning action values Q Ï is useful in practice because we can use them to directly derive an optimal policy using Equation (18.10). level, Monte Carlo methods for estimating action value functions Q Ï (x, u) are essentially equivalent to the method presented above for estimating state values, with the only difference being that we now consider visits to state-action pairs in- stead of states. We consider a state-action pair to have been visited in an episode if the agent is in state x and takes action u at some point during the episode. As with state-value estimation, Monte Carlo methods estimate the action-value function Q Ï (x, u) by averaging the cumulative rewards observed from visits to the stateâaction pair (x, u). However, estimating action values introduces an ad- ditional challenge. Suppose that experience is generated using a deterministic policy Ï. Under such a policy, the agent selects the same action whenever it encounters a given state and therefore observes rewards for only one action per state. As a result, the agent receives no information about the values of alternative actions in that state, making it impossible to compare actions and improve the policy. This challenge is known as the problem of maintaining exploration and is central to reinforcement learning. As we will see throughout the remainder of this chapter, a common strategy for addressing this issue is to employ policies that ensure every stateâaction pair is visited with nonzero probability. One way to achieve 470 principles of robot autonomy this is by using stochastic policies that assign positive probability to all actions in each state, thereby guaranteeing sufficient exploration of the environment. 18.3.2 Temporal-Difference Learning Temporal-difference (TD) learning is widely considered one of the most influential concepts in reinforcement learning. At a high level, temporal-difference learn- ing combines elements of both Monte Carlo methods and dynamic program- ming. Like dynamic programming methods, temporal-difference learning up- dates value estimates using other learned estimates rather than waiting for com- plete returns. This mechanism is known as bootstrapping and allows learning from incomplete sequences of experience 13 At the same time, like Monte Carlo meth- 13 In other words, without waiting for the end of an episode. ods, temporal-difference learning learns directly from sampled experience and does not require an explicit model of the environment: a property referred to as sampling. By combining bootstrapping with sampling, temporal-difference learn- ing effectively bridges the gap between Monte Carlo and dynamic programming methods, inheriting many of the advantages of both approaches. Similar to Monte Carlo methods, temporal-difference methods address the pre- diction problem by collecting samples from the environment and using them to update value estimates. Recall that in Monte Carlo methods, we must wait un- til the end of the episode to compute the cumulative reward following time t, which we denote as G t , and then use G t to define the target for the value function update: V(x t )â V(x t ) + α(G t â V(x t )),(18.16) where â denotes the assignment operator and α is an externally-specified step- size parameter 14 . In contrast, temporal-difference methods update the value func- 14 In Algorithm 18.3, we used α = 1/N(x t ) tion estimate at each time step t based on the observed reward r t and the estimate of the value function at the next state V(x t+1 ) by the update: V(x t )â V(x t ) + α(r t + ÎłV(x t+1 )â V(x t )).(18.17) This update rule is known as the TD(0) update, where the subscript 0 indi- cates that the update relies on a single step of experience 15 . By comparing 15 TD(0) is a special case of the more gen- eral TD(λ) family of methods. the Monte Carlo and temporal-difference update rules, we observe that comput- ing the Monte Carlo target G t requires access to an entire episode, whereas the temporal-difference target r t + ÎłV(x t+1 ) can be computed immediately at each time step t. We provide a complete algorithm for TD(0) in Algorithm 18.4. It is worth noting that the quantity r t + ÎłV(x t+1 )â V(x t ) can be interpreted as an error that measures the discrepancy between the current estimate of the value function, V(x t ), and an improved target estimate, r t + ÎłV(x t+1 ) 16 . This 16 This target is more informative be- cause it incorporates the realized reward r t from the transition x t â x t+1 , together with the current estimate of the value of the successor state. quantity, known as the TD error, plays a crucial role in the development of many reinforcement learning algorithms. reinforcement learning 471 Algorithm 18.4: Temporal-Difference Learning (TD(0)) Data: Initial value function, V, arbitrarily initialized for all xâX . Result: Value function estimate, V Ï . for each episode do Initialize state x 0 for t = 0, 1, . . . until x t is terminal do Take action u t according to Ï Observe r t and x t+1 V(x t )â V(x t ) + α(r t + ÎłV(x t+1 )â V(x t )) return V â V Ï 18.3.3 Example: Monte Carlo Control We have already discussed how we can use the Monte Carlo and temporal- difference learning paradigms to address the problem of learning without a model of the environment. However, we solely discussed these methods in the context of the prediction problem, where we are trying to estimate the value function of a given policy. In this section, we introduce our first complete example of a rein- forcement learning algorithm for learning optimal policies through Monte Carlo methods. Later in this chapter, we introduce various reinforcement learning al- gorithms that, in one way or another, build upon the principles of model-free control that we discuss here. The central idea behind using Monte Carlo methods for control mirrors the principles underlying policy iteration, as introduced in Section 18.2. We use the term Generalized Policy Iteration (GPI) to refer to the broad framework encompass- ing all methods that alternate between policy evaluation and policy improvement. To illustrate this idea, consider a straightforward Monte Carlo extension of the classical policy iteration algorithm. Starting from an arbitrary initial policy Ï 0 , the algorithm alternates between two phases: policy evaluation, in which the value function of the current policy is estimated from sampled experience, and policy improvement, in which the policy is updated based on these value estimates. This process is repeated until convergence. As discussed in Section 18.3.1, learning action-value functions is often more convenient than learning state-value functions, since action values can be used directly to derive policies. Accordingly, in this example we focus on learning the action-value function. At a high level, the algorithm proceeds by repeatedly alternating between the following steps: Ï 0 E ââ Q Ï 0 I ââ Ï 1 E ââ Q Ï 1 I ââ Ï 2 E ââ . . . I ââ Ï â E ââ Q Ï â , where E ââ denotes policy evaluation, and I ââ denotes policy improvement. In contrast to the Policy Iteration algorithm, we use the Monte Carlo prediction ap- proach from Section 18.3.1 in the policy evaluation step rather than using the ex- act Bellman equation to update the value function, which would require a model 472 principles of robot autonomy of the environment. The policy improvement step remains the same, where we define the new policy by acting greedily with respect to the current action-value function by choosing Ï i+1 (x) = arg max u Q Ï i (x, u). While this algorithm captures several core principles underlying many rein- forcement learning algorithms, it remains relatively simplistic. As discussed in earlier sections, learning accurate action-value estimates from experience requires the agent to maintain sufficient exploration. More formally, the behavior policy must ensure that every stateâaction pair is visited with nonzero probability. In the formulation presented above, however, no explicit exploration mechanism is incorporated to guarantee this condition. As a result, the algorithm is only valid under the exploring starts assumption, which posits that each episode begins in every possible stateâaction pair with nonzero probability. Although useful for theoretical analysis, this assumption is generally unrealistic in practical applica- tions. To obtain a practical and broadly applicable algorithm, this assumption must be relaxed. In the following sections, we introduce several strategies for ensuring adequate exploration, enabling effective learning from experience in re- alistic settings. 18.3.4 A Unifying View of Reinforcement Learning Temporal-difference learning Dynamic programming Monte Carlo methods Exhaustive search Sample-based Exact Bootstrapping Episodes Figure 18.2: We can categorize reinforcement learning meth- ods along two-axes based on whether they are sample-based and whether they bootstrap. Monte Carlo, temporal-difference, and dynamic programming methods are often presented as distinct approaches to reinforcement learning. However, it is worth noting that these methods are extremes of a spectrum. To appreciate this, we can consider the advantages, disadvantages, and commonalities of each of these paradigms. Monte Carlo and temporal-difference methods have an advantage over dy- namic programming methods in that they do not require an explicit model of the environment and can learn directly from interaction with itâthat is, they rely on sampling. This property greatly extends the applicability of these methods to real-world problems, where the environment could be unknown or too complex reinforcement learning 473 to model. Temporal-difference and dynamic programming methods, in turn, offer a key advantage over Monte Carlo methods in that they update value estimates with- out waiting for an episode to terminate and can therefore learn from incomplete sequences of experience through bootstrapping. This capability is particularly im- portant in practice, as many real-world tasks involve very long or even non- terminating episodes. On the other hand, Monte Carlo methods also possess an advantage over temporal-difference methods in that they provide unbiased estimates of the true value function.This is because Monte Carlo updates rely on the actual re- turn G t , whereas temporal-difference methods update toward an estimated tar- get, R(x t , u t ) + ÎłV(x t+1 ). By doing so, temporal-difference methods deliberately trade some biasâarising from bootstrapped targetsâfor a reduction in variance, since these targets depend on fewer sources of stochasticity. These relationships are summarized in Section 18.3.4, which organizes rein- forcement learning methods along two dimensions: the use of samples and the degree of bootstrapping. At the top right lies exhaustive search, where quantities of interestâsuch as value functionsâare computed exactly through model-based simulation of all possible system evolutions. At the bottom right are dynamic programming methods, which leverage the principle of optimality and a known model to perform one-step lookahead updates via bootstrapping. Moving left- ward relaxes the requirement of a model or exhaustive computation, replacing it with learning from sampled experience. At the top left are Monte Carlo meth- ods, which avoid exhaustive search by learning from complete episodes. Finally, at the bottom left are temporal-difference methods, which combine the sampling of Monte Carlo methods with the bootstrapping of dynamic programming, en- abling efficient learning from incomplete sequences of experience. 18.4 A Taxonomy of Reinforcement Learning Over the last years, the field of reinforcement learning has seen a rapid growth in the number of algorithms and methods, each with its own strengths and weak- nesses. While an exhaustive treatment of all these methods is beyond the scope of this chapter, we aim to provide an overall picture of the different types of al- gorithms that exist, a deeper understanding of the core principles that underlie these algorithms, and a number of representative examples from each category. In this section, we provide a birdâs-eye view of the field of reinforcement learn- ing and classify the different algorithms into a taxonomy, shown graphically in Section 18.4, that can serve as reference through the rest of the chapter. The first fundamental distinction among reinforcement learning algorithms is between model-free and model-based methods. Model-free methods, which are the focus of Section 18.5, attempt to learn the optimal policy directly from experi- ence, without explicitly modeling the environment. Model-based methods, on the other hand, aim to learn a model of the environment and then use this model 474 principles of robot autonomy Reinforcement Learning Model-freeModel-based Learned ModelKnown Model Policy Optimization Value-based Figure 18.3: A taxonomy of re- inforcement learning algorithms based on whether or not they leverage a model of the environ- ment. to derive optimal policies. We cover model-based methods in Section 18.6. Looking at model-free methods, we can further distinguish between value-based and policy optimization methods. Value-based methods, similar to the ones de- scribed in previous sections, define the policy implicitly through a value function. By doing so, the main focus of value-based methods lies in accurately estimating the optimal value function, which we can then use to derive the optimal policy. Policy optimization methods, on the other hand, represent the policy explicitly via a parametric function and optimize the parameters of this function to maxi- mize the reinforcement learning objective. On the other side of the graph in Section 18.4, we can see how model-based methods can be further divided into algorithms that either focus on learning a model of the environment from data, or algorithms that use a known model 17 to 17 For example, the model could be known from physics or other domain knowledge. derive optimal policies. In this chapter, we focus on methods that learn a model of the environment from data. As we will see in Section 18.6, learned models can be used in various ways, such as within planning routines or for accelerating model-free algorithms. 18.4.1 On-policy vs Off-policy Learning Another crucial distinction among reinforcement learning algorithms is between on-policy and off-policy learning. On-policy methods aim to evaluate or improve the policy that is used to interact with the environment. Off-policy methods, on the other hand, evaluate or improve a policy that is different from the one used to interact with the environment. For example, the Monte Carlo control algorithm we presented in Section 18.3.3 is an on-policy method, as it attempts to improve the same policy used to make decisions in the environment. Off-policy methods define two distinct policies: one that must be updated and ideally becomes the optimal policy and one used to interact with the environment. We refer to the policy being learned as the target policy, while we refer to the policy used to generate samples from the environment as the behavior policy. In general, on-policy methods are simpler to implement and tend to be more reinforcement learning 475 reliable than off-policy methods. Off-policy approaches, on the other hand, are typically more complex and require additional algorithmic considerations, which can make the learning phase less stable and lead to slower convergence in prac- tice. Off-policy learning enables agents to learn from data generated by arbitrary behavior policies, including data collected by human demonstrators or by con- ventional, non-learning-based controllers. Moreover, off-policy methods make it possible to learn about multiple policies simultaneously, or to learn an optimal policy while continuing to explore the environment. Throughout the next sections, we introduce various on-policy and off-policy methods for learning and discuss the advantages and disadvantages of each ap- proach. 18.5 Model-free Reinforcement Learning Model-free reinforcement learning methods are commonly considered to be the most popular and widely used class of algorithms in the field. This popularity is largely due to their recent successes in a wide range of applications, including playing games 18 , robot control 19 , and the fine-tuning of large-scale AI chatbots 20 . 18 D. Silver et al. âMastering the game of Go with deep neural networks and tree searchâ. In: Nature 529.7587 (2016), p. 484â489 19 S. Levine et al. âEnd-to-End Training of Deep Visuomotor Policiesâ. In: Jour- nal of Machine Learning Research 17.39 (2016), p. 1â40 20 Y. Bai et al. âTraining a Helpful and Harmless Assistant with Reinforcement Learning from Human Feedbackâ. In: (2022). url: https://arxiv.org/abs/ 2204.05862 In this section, we discuss representative reinforcement learning algorithms while maintaining our focus on key principles important for understanding and imple- menting new algorithms. 18.5.1 Value-based Methods We begin our discussion on model-free reinforcement learning with value-based methods, which, similarly to the Monte Carlo control algorithm presented in Section 18.3.3, estimate the value function of a policy and use this estimate to derive the optimal policy. Q-learning. One of the first breakthroughs in reinforcement learning was the introduction of the Q-learning algorithm 21 . Q-learning is an off-policy algorithm 21 C. J. C. H. Watkins and P. Dayan. âQ-learningâ. In: Machine Learning 8.3 (1992), p. 279â292 that learns the optimal action-value function Q â (x, u) via TD learning. Because Q-learning is off-policy, it estimates the optimal action value function Q â (x, u) independently of the behavior policy used to interact with the environment. The behavior policy therefore does not determine the objective of learning, but instead governs which stateâaction pairs are explored and, consequently, which action- value estimates are updated. Provided that the behavior policy is sufficiently exploratory 22 , Q-learning is guaranteed to converge to the optimal action-value 22 That is, all stateâaction pairs are vis- ited infinitely often in the limit. function Q â (x, u). Given a transition sampled from the environment, (x, u, r, x âČ ), Q-learning up- dates the action-value estimate Q(x, u) according to: Q(x, u)â Q(x, u) + α r + Îł max u âČ Q(x âČ , u âČ )â Q(x, u) ,(18.18) 476 principles of robot autonomy where α > 0 is a tunable learning rate parameter. Importantly, regardless of the behavior policy used to generate the transition (x, u, r, x âČ ), the update in Equation (18.18) adjusts the current estimate Q(x, u) toward the action value induced by the greedy policy with respect to the current Q-function, namely r + Îł max u âČ Q(x âČ , u âČ ). In this sense, Q-learning repeatedly enforces the Bellman optimality equation for the action-value function until convergence 23 . From this 23 As discussed in Section 18.2.4, the Bell- man optimality operator has a unique fixed point, which corresponds to the optimal action-value function Q â (x, u). perspective, Q-learning can be viewed as a sample-based approximation of Q- value iteration 24 , in which the Bellman optimality update is performed using a 24 That is, value iteration applied to action-value functions rather than state- value functions. single transition sample. To ensure that the behavior policy is sufficiently exploratory, Q-learning typ- ically employs an Δ-greedy action-selection strategy. Despite its simplicity, the Δ-greedy policy approach is an effective and widely used exploration mechanism where, with probability 1â Δ, the agent selects the action that is greedy with respect to the current Q-function, while with probability Δ it selects an action uniformly at random. This strategy ensures that Ï(u | x) > 0 for all states x and actions u, which is a key requirement for the convergence of Q-learning. We present the complete Q-learning algorithm in Algorithm 18.5. Algorithm 18.5: Q-Learning Data: Initial action values, Q(x, u), learning rate, α, discount factor, Îł, exploration rate, Δ. Result: Updated action values, Q(x, u). for each episode do Initialize the state, x 0 . for each step in the episode do Select an action, u t , using an Δ-greedy policy with respect to Q(x t , u). Execute the action u t and observe the reward, r t , and the next state, x t+1 . Update the action-value function using the transition (x t , u t , r t , x t+1 ). Q(x t , u t )â Q(x t , u t ) + α ( r t + Îł max u âČ Q(x t+1 , u âČ )â Q(x t , u t ) ) return Q(x, u)â Q â (x, u) Value Function Approximation. So far, we have assumed that the state and action spaces, X and U , are finite and small enough to be easily stored in a look-up table and to allow for meaningful state-action space exploration within a reason- able computation budget. However, in robotics applications, the state and action spaces are often continuous or extremely high-dimensional, making it imprac- tical to compactly store and efficiently update value functions. This challenge motivates us to consider methods that rely on parametrized function approximation. Within this class of methods, we represent value functions by a parametric func- tion, Q Ξ (x, u), or alternatively V Ξ (x), with parameters Ξ. The goal of learning is reinforcement learning 477 then to find the optimal parameters Ξ â , such that the value function estimator is close to the optimal value function. Function approximation has two key advantages. First, it allows us to repre- sent value functions compactly, as the number of parameters Ξ is typically much smaller than the number of states and actions. Second, it enables the value func- tion estimator to generalize to unseen states and actions, potentially reducing the amount of exploration required to learn a good policy 25 . While there are 25 In other words, if the function approx- imator is able to generalize successfully, the agent does not need to visit every state-action pair. many choices for the function approximator, including linear functions, neural networks, and decision trees, we focus our discussion on differentiable functions. Given a dataset, D, of transitions (x, u, r, x âČ ), policy evaluation via function approximation entails learning the parameters Ξ that minimize the loss function J(Ξ): J(Ξ) = E (x,u,r,x âČ )âŒD [ Q Ï (x, u)â Q Ξ (x, u) ] .(18.19) Intuitively, J(Ξ) measures the discrepancy between the estimated value function, Q Ξ (x, u), and the target value function, Q Ï (x, u), where Q Ï (x, u) is the value function under the policy Ï that generated the transitions in D. This optimiza- tion problem can be solved via stochastic gradient descent, which defines the following update rule: Ξâ Ξ +â Ξ ,(18.20) where: â Ξ = α ( Q Ï (x, u)â Q Ξ (x, u) ) â Ξ Q Ξ (x, u). Here, â Ξ Q Ξ (x, u) denotes the gradient of the value function estimator with re- spect to its parameters Ξ, and α > 0 is the learning rate. In practice, however, the update rule in Equation (18.20) is not directly appli- cable since the true target value function Q Ï (x, u) is unknown. Leveraging ideas from model-free control, the unknown target can be replaced by an estimate de- rived from Monte Carlo, temporal-difference, or dynamic programming methods. For example, using a Monte Carlo estimate, the parameter update can be written as: â Ξ = α ( G t â Q Ξ (x t , u t ) ) â Ξ Q Ξ (x t , u t ),(18.21) where G t = â Tâ1 k=t Îł kât r k denotes the return observed from time step t. Alter- natively, a temporal-difference approach replaces the Monte Carlo return with a one-step bootstrap estimate, yielding the update: â Ξ = α ( r t + ÎłQ Ξ (x t+1 , u t+1 )â Q Ξ (x t , u t ) ) â Ξ Q Ξ (x t , u t ).(18.22) In both cases, the Monte Carlo and temporal-difference targets are computed from trajectories generated by applying the behavior policy in the environment. Fitted Q-learning. A particularly popular algorithm that combines function ap- proximation with temporal-difference learning is the Fitted Q-Learning algorithm. 478 principles of robot autonomy Fitted Q-Learning updates the parameters Ξ of a Q-function estimator by apply- ing the update rule in Equation (18.20) with: â Ξ = α r t + Îł max u âČ Q Ξ (x t+1 , u âČ )â Q Ξ (x t , u t ) â Ξ Q Ξ (x t , u t ).(18.23) It is important to note that the target r t + Îł max u âČ Q Ξ (x t+1 , u âČ ) is equivalent to the temporal-difference target used in Q-learning. Essentially, Fitted Q-Learning mimics the update rule of Q-learning, but rather than directly updating the ac- tion values to explicitly enforce the Bellman optimality equation it updates the parameters Ξ of the Q-function estimator to approximately enforce it. In other words, rather than updating the entries of a look-up table representing the value function, Fitted Q-learning updates the parameters Ξ to minimize the error with respect to the fixed point of the Bellman optimality operator. In this section, we discussed a few foundational examples of value-based rein- forcement learning methods. While these are only a subset of the vast literature in value-based methods, they convey the key ideas and challenges of learning value functions for control. Specifically, these methods highlight the central idea of approximating value functions from experience and leveraging them to derive optimal policies. Key challenges include ensuring sufficient exploration, which we can address through strategies like Δ-greedy policies, and addressing stability and convergence of the learning process when using value function approxima- tors, particularly in high-dimensional or continuous spaces. Value-based meth- ods in reinforcement learning are fundamentally built on a concise set of core principles, such as generalized policy iteration and (approximate) value iteration, and differ primarily in their usage of value update targets 26 , function approxima- 26 For instance, Monte Carlo, temporal- difference, or dynamic programming ap- proaches. tors, or behavior policies. Therefore, understanding these foundational concepts provides us a lens through which we can interpret the majority of value-based algorithms. 18.5.2 Policy Optimization Methods We now continue our discussion of model-free reinforcement learning by turn- ing to policy optimization (PO) methods. In contrast to value-based approaches, policy optimization methods adopt a fundamentally different strategy for solv- ing the reinforcement learning problem by directly optimizing the policy itself. To better motivate policy optimization, let us recall the reinforcement learning objective from Section 18.1: V Ï = E Ï⌠p Ï (Ï) " Tâ1 â t=0 Îł t r t # . Rather than learning a value function and deriving a policy from it, as in value- based methods, policy optimization methods define a parametric policy, Ï Îž , and directly optimize the parameters Ξ to maximize the reinforcement learning objec- tive V Ï . Formally, the goal of policy optimization is to find the optimal policy reinforcement learning 479 parameters: Ξ â = arg max Ξ V(Ξ),(18.24) where, for simplicity, we use V(Ξ) to refer to the reinforcement learning objective, V Ï Îž , under policy Ï Îž with parameters Ξ. Policy optimization methods typically address this problem in two stages. First, they estimate the gradient of the objective with respect to the policy param- eters, â Ξ V(Ξ). Then, they update the parameters by performing (approximate) gradient ascent: Ξâ Ξ + αâ Ξ V(Ξ),(18.25) where α is a user-defined learning rate. The first challenge that all policy optimization methods face is estimating the gradient of the reinforcement learning objective. To simplify the notation, we define the cumulative reward as R(Ï) = â Tâ1 t=0 Îł t r t and assume Îł = 1 27 . By 27 The extension to the discounted case is equivalent and relatively straightfor- ward. definition of expectation, we have: V(Ξ) = E Ï⌠p Ξ (Ï) [ R(Ï) ] = Z p Ξ (Ï)R(Ï)dÏ,(18.26) where p Ξ (Ï) denotes the trajectory distribution induced by the policy Ï Îž . Equa- tion (18.26) allows us to write the gradient of the reinforcement learning objective as: â Ξ V(Ξ) =â Ξ Z R(Ï) p Ξ (Ï)dÏ = Z â Ξ p Ξ (Ï)R(Ï)dÏ.(18.27) However, we cannot compute this gradient directly because it depends on un- known dynamics through the trajectory distribution, p Ξ (Ï) 28 . 28 Recall that we do not assume access to the system dynamics and thus cannot compute p Ξ (Ï) explicitly. To address this issue, we resort to the following useful identity: p Ξ (Ï)â Ξ log p Ξ (Ï) = p Ξ (Ï) â Ξ p Ξ (Ï) p Ξ (Ï) =â Ξ p Ξ (Ï),(18.28) and use it to rewrite the gradient of the reinforcement learning objective in Equa- tion (18.27) as: â Ξ V(Ξ) = Z â Ξ p Ξ (Ï)R(Ï)dÏ = Z p Ξ (Ï)â Ξ log p Ξ (Ï)R(Ï)dÏ = E Ï⌠p Ξ (Ï) [ â Ξ log p Ξ (Ï)R(Ï) ] . We can then approximate the expectation using Monte Carlo methods by sam- pling from the trajectory distribution, p Ξ (Ï), through interaction with the en- vironment. However, the gradient of the log-probability, â Ξ log p Ξ (Ï), remains intractable to compute directly. To address this, let us recall the definition of the trajectory distribution, p Ξ (Ï): p Ξ (Ï) : = p(x 0 ) Tâ1 â t=0 p(x t+1 | x t , u t )Ï Îž (u t | x t ),(18.29) 480 principles of robot autonomy where taking the logarithm yields: log p Ξ (Ï) = log p(x 0 ) + Tâ1 â t=0 log p(x t+1 | x t , u t ) + log Ï Îž (u t | x t ).(18.30) By substituting Equation (18.30) into the gradient of the reinforcement learning objective, we obtain: â Ξ V(Ξ) = E Ï⌠p Ξ (Ï) [ â Ξ log p Ξ (Ï)R(Ï) ] = E Ï⌠p Ξ (Ï) " â Ξ log p(x 0 ) + Tâ1 â t=0 log p(x t+1 | x t , u t ) + log Ï Îž (u t | x t ) ! R(Ï) # . Notably, the terms log p(x 0 ) and log p(x t+1 | x t , u t ) do not depend on Ξ, and thus can be ignored when computing the gradient of the reinforcement learning objective. Moreover, evaluating the gradient of the log-probability of the action, log Ï Îž (u t | x t ), is tractable and we can easily compute it, for example by using automatic differentiation tools. This leads to the following expression for the gradient of the reinforcement learning objective: â Ξ V(Ξ) = E Ï⌠p Ξ (Ï) " Tâ1 â t=0 â Ξ log Ï Îž (u t | x t )R(Ï) # ,(18.31) which is tractable to compute and which we can estimate using samples from the environment. For example, given N episodes of interaction with the environment, we can estimate the gradient of the reinforcement learning objective as: â Ξ V(Ξ)â 1 N N â i=1 Tâ1 â t=0 â Ξ log Ï Îž (u i t | x i t )R(Ï i ).(18.32) This is a crucial result and lays the foundations for almost all policy optimization algorithms in reinforcement learning. REINFORCE. The derivations above directly lead to one of earliest examples of policy optimization methods 29 known as the REINFORCE algorithm 30 . At a 29 In the reinforcement learning litera- ture, these are often also referred to as policy gradient methods. 30 R. J. Williams.âSimple statistical gradient-following algorithms for con- nectionist reinforcement learningâ. In: Machine Learning 8.3 (1992), p. 229â256 high level, the REINFORCE algorithm estimates the gradient of the reinforce- ment learning objective in Equation (18.32) using samples from the environment, and after each episode updates the policy parameters Ξ in the direction of the gradient. We outline the REINFORCE algorithm in Algorithm 18.6. From Equation (18.31), we can see that the gradient is computed as the sum of the gradients of the log-probabilities of the actions, weighted by the return of the trajectory. Although this result follows from a mathematical derivation, it also admits a clear and intuitive interpretation. By updating the policy parameters in the direction of the policy gradient, the algorithm increases the log-probability of actions that lead to high returns while decreasing the log-probability of actions that lead to low returns. In this way, the policy gradient formalizes the notion reinforcement learning 481 Algorithm 18.6: REINFORCE Algorithm Data: Initial policy parameters, Ξ, learning rate, α Result: Update policy parameters, Ξ for each episode do Generate a trajectory, Ï =x 0 , u 0 , r 0 , . . . , x T , using the policy Ï Îž in the environment. Compute the return, R(Ï) = â Tâ1 t=0 r t . Ξâ Ξ + αâ Ξ V(Ξ), whereâ Ξ V(Ξ) is computed by Equation (18.32). return Ξ of trial-and-error learning, where behaviors that prove effective are reinforced, whereas ineffective behaviors are gradually suppressed. Policy optimization methods represent a popular and intuitive approach to reinforcement learning and have several advantages and disadvantages compared to value-based methods. A first key advantage of policy optimization methods is that they can naturally handle both discrete and continuous action spaces because we can compute the gradient of the policy using automatic differentiation tools 31 . 31 Assuming the policy is parameterized by a differentiable function. For example, in the case of continuous action spaces, we can parameterize the policy as a Gaussian distribution, and all of the derivations developed in this section would still hold. Additionally, policy optimization methods have the notable advantage of directly optimizing the reinforcement learning objective. This ensures that improvements to the policy are measured against a well-defined metric, since better values of the reinforcement learning objective imply a better policy. In contrast, value-based methods rely on fixed-point iterations of value functions to satisfy the Bellman equation with the goal to eventually converging to the optimal value function. While this approach is theoretically sound, it is unclear how suboptimal the policy is during intermediate iterations. Policy gradient methods also have some disadvantages. First, the policy opti- mization methods presented so far are inherently on-policy methods, which can result in high sample inefficiency 32 . A second disadvantage is that the gradi- 32 A number of off-policy policy opti- mization algorithms have been intro- duced to allow the policy to be updated using experiences collected from differ- ent policies. These methods aim to ap- proximate the behavior of classical on- policy algorithms while improving sam- ple efficiency. Despite this advantage, they often introduce additional complex- ities, such as the need for more sophis- ticated exploration strategies and man- aging the stability of the off-policy up- dates. ent defined in Equation (18.32) is a high-variance estimator of the true gradient from Equation (18.31). In practice, this can lead to extremely noisy updates and therefore slow convergence. As we will see in the remainder of this section, a lot of research in the domain of policy optimization has focused on addressing these limitations to develop sample-efficient and lower-variance policy gradient estimates. Actor-Critic Methods. Actor-critic methods represent an important extension of policy optimization that reduces the high variance associated with policy gradient estimates. Let us recall the definition of the policy gradient and slightly rearrange the summation terms to highlight the sum over future rewards 33 : 33 Where again, for simplicity, we assume Îł = 1. â Ξ V(Ξ) = 1 N N â i=1 Tâ1 â t=0 â Ξ log Ï Îž (u i t | x i t ) Tâ1 â t âČ =t r i t âČ ! . 482 principles of robot autonomy We refer to the term â Tâ1 t âČ =t r i t âČ as the reward-to-go. The reward-to-go is a one- sample estimate of the true return, which is defined as the expected cumulative reward under the trajectory distribution, E Ï⌠p Ξ (Ï) h â Tâ1 t âČ =t r t âČ i . While conceptu- ally straightforward, this reward-to-go estimate introduces significant variance, leading to noisy policy gradient updates. Actor-critic methods address this challenge by introducing a critic, which is a parametric approximation of the value function. Since the value function esti- mates the expected reward-to-go, the critic enables us to replace the high-variance sample-based estimate with a lower-variance, learned approximation. Concretely, the policy gradient becomes: â Ξ V(Ξ)â 1 N N â i=1 Tâ1 â t=0 â Ξ log Ï Îž (u i t | x i t )Q Ï (x i t , u i t ),(18.33) where Q Ï (x t , u t ) is the criticâs estimate of the action-value function. We can up- date the critic using any value estimation method, such as the value-based meth- ods with function approximation discussed in Section 18.5.1. A particularly popular choice for the definition of the policy gradient in actor- critic methods is through the advantage function: A Ï (x t , u t ) = Q Ï (x t , u t )â V Ï (x t ),(18.34) which quantifies the relative merit of taking action u t in state x t , compared to the average value of the state. Intuitively, if the policy gradient in the REINFORCE algorithm can be thought of as a way to increase the probability of good actions, the advantage function can be thought of as a way to increase the probability of actions that are better than the average. This normalization of the policy gradient through the advantage defines the so-called Advantage Actor Critic (A2C) 34 algo- 34 V. Mnih et al. âAsynchronous Meth- ods for Deep Reinforcement Learningâ. In: Proceedings of The 33rd International Conference on Machine Learning. 2016, p. 1928â1937 rithm, which significantly reduces variance and improves learning stability. In practice, we often approximate the advantage function to avoid estimating both Q Ï and V Ï , using: A Ï (x t , u t )â r t + ÎłV Ï (x t+1 )â V Ï (x t ).(18.35) Actor-critic methods typically involve iterative updates of both the policy pa- rameters, Ξ, and the value function parameters, Ï. At each step, we improve the policy based on the estimated advantage and we refine the value function to better approximate future rewards. The pseudocode for A2C is presented in Algorithm 18.7 and highlights these alternating updates. Actor-critic methods blend the strengths of policy optimization and value- based approaches, achieving a balance between expressive policy representations and efficient variance reduction. However, the introduction of a critic also adds computational complexity and tuning challenges that we must consider in prac- tical implementations. reinforcement learning 483 Algorithm 18.7: Advantage Actor Critic (A2C) Data: Initial policy parameters, Ξ, and value function parameters, Ï, learning rates, α Ξ , α Ï , discount factor, Îł Result: Updated policy parameters, Ξ for each episode do Sample trajectories, Ï =x 0 , u 0 , r 0 , . . . , x T , using Ï Îž . for t = 0 to Tâ 1 do Compute the value target, for example using temporal-difference: y t = r t + ÎłV Ï Ï (x t+1 ). Ïâ Ï + α Ï â Ï (y t â V Ï Ï (x t )) 2 A Ï Ï (x t , u t ) = y t â V Ï Ï (x t ) Ξâ Ξ + α Ξ â Ξ log Ï Îž (u t | x t )A Ï Ï (x t , u t ) return Ξ 18.5.3 Limitations of Model-free Reinforcement Learning Despite their successes, model-free reinforcement learning methods face several important challenges. A primary limitation is sample efficiency. Model-free al- gorithms often require a large number of interactions with the environment to learn an effective policy, which can be prohibitively expensive or impractical in many real-world settings, particularly in robotics. A second limitation is that, in their standard formulation, model-free methods are inherently single-task learn- ers. Given a fixed reward function defining a specific task, these methods learn a policy optimized exclusively for that task, making it difficult to reuse knowledge or transfer learned behaviors across tasks. Finally, many real-world applications provide reward signals that are sparse, delayed, or noisy. Such reward structures significantly represent a challenge to the learning process, as useful feedback may be infrequent or difficult to attribute to specific actions. As a result, model-free methods can struggle to discover effective policies in these environments. 18.6 Model-based Reinforcement Learning Model-based reinforcement learning methods aim to address the limitations of model-free methods by learning a model of the environment. In this section, we introduce two broad classes of model-based reinforcement learning methods: model-based planning methods that learn a model and use it to plan and model-based policy optimization methods that learn a model and use it to accelerate model-free policy learning. 18.6.1 Model-based Planning If we had access to a model of the dynamics, p(x t+1 | x t , u t ), we could directly leverage tools from model-based optimal control to compute an optimal action sequence or policy. Motivated by this observation, the central idea behind model- 484 principles of robot autonomy based planning methods is to learn an approximate model of the environment dynamics from data, and then use this learned model to plan. A generic model-based planning procedure can be summarized as follows: 1. Run a base policy, Ï 0 , in the environment and collect a dataset of transitions, D =(x t , u t , r t , x t+1 ). 2. Fit a dynamics model, p Ξ (x t , u t ), to the observed data to minimize the pre- diction error, for example by minimizing the mean squared error between the predicted and true next state: minimize Ξ â (x t ,u t ,r t ,x t+1 )âD â„ p Ξ (x t , u t )â x t+1 â„ 2 . 3. Use the learned dynamics model to plan a sequence of actions for the agent to execute. Despite its simplicity, this scheme works for relatively well-behaved systems, where the datasetD guarantees sufficient coverage of the state-action space, and where the learned model p Ξ is accurate enough to enable effective planning 35 . 35 This scheme is essentially equivalent to a task known as system identification. However, in practice, learning an accurate model of the dynamics is often chal- lenging, especially when dealing with high-dimensional, non-linear, and stochas- tic systems. Most importantly, inaccuracies in the learned model are particularly problematic when used within an optimization process. Optimization algorithms will naturally exploit inaccuracies in the model that have high predicted perfor- mance, but may not correspond to high realized performance. A popular approach to address this issue is to consider a measure of uncer- tainty in the modelâs predictions, and to use this uncertainty to inform the plan- ning process. While there are many ways to quantify uncertainty, we consider methods that aim to learn a posterior distribution over the model parameters. In these methods, rather than learning a single estimate of the model parameters Ξ through standard maximum likelihood estimation: Ξ â = arg max Ξ log p Ξ (D | Ξ),(18.36) where p Ξ (D | Ξ) is the likelihood of the data given the model parameters, we instead aim to learn a posterior distribution, p(Ξ|D), over the model parameters. In other words, we aim to learn a full distribution over the model parameters that is consistent with the observed data, potentially capturing multiple plausible models that explain the data, and ultimately enabling us to reason about the uncertainty in the modelâs predictions. We can achieve this by applying Bayesâ rule to compute the posterior distribution: p(Ξ|D) = p(D | Ξ) p(Ξ) p(D) ,(18.37) where p(D | Ξ) is the likelihood of the data given the model parameters, p(Ξ) is the prior distribution over the model parameters, and p(D) is the marginal reinforcement learning 485 likelihood of the data. While a complete treatment of Bayesian inference is beyond the scope of this book, we refer the interested reader to standard textbooks on the subject, such as by Murphy [100]. For the purpose of this chapter, it will be sufficient to assume that Bayesian inference provides us with computational methods to derive an estimate of the posterior distribution, which we can then use to inform the planning process. Once we have an estimate of the posterior distribution over the model param- eters, we can apply it in the following model-based planning scheme: 1. Run a base policy, Ï 0 , in the environment and collect a dataset of transitions, D =(x t , u t , r t , x t+1 ). 2. Use D to estimate a posterior distribution, p(Ξ | D), over the model parame- ters. 3. Sample a set of K plausible models,Ξ 1 , . . . , Ξ K ⌠p(Ξ|D). 4. For each sampled model, Ξ k , and given a candidate action plan, (u 1 , . . . , u Tâ1 ), use the model to compute the expected return: V(u 1 , . . . , u Tâ1 ) = 1 K K â k=1 Tâ1 â t=1 R(x t , u t ), where x t+1 ⌠p Ξ k (x t+1 | x t , u t ). 5. Execute the first action from the best plan according to the expected return. This scheme allows us to leverage the uncertainty in the modelâs predictions by considering multiple plausible models and to reason about the expected return under each model rather than optimizing under a single model. Despite its effectiveness, it is important to note that this scheme is just a high- level description of the model-based planning process, and there are many prac- tical considerations that we would need to address to make this approach work in practice. 18.6.2 Model-based Policy Optimization The second class of model-based reinforcement learning methods we consider is model-based policy optimization. In contrast to model-based planning, which uses the learned model to plan a sequence of actions, model-based policy optimization uses the learned model to improve model-free policy learning. Specifically, having a learned model allows us to consider two sources of expe- rience: real-world data collected by executing the policy in the environment and synthetic data generated by the model. Given an MDP, M = (S ,A, p, R, Îł), and a learned model, p Ξ (x t+1 , r t | x t , u t ) 36 , we can consider two sources of experience: 36 Here we consider the general case where we learn both the next state and the reward, but we can extend the dis- cussion to the case where we only have to learn one of the two. (x t , u t , r t , x t+1 )⌠Environment data, (x t , u t , Ë r t , Ëx t+1 )⌠p Ξ (x t+1 , r t | x t , u t ). 486 principles of robot autonomy The basic idea of model-based policy optimization is to use both sources of experience to improve model-free policy learning. One of the earliest and most popular methods in this category is the Dyna-Q algorithm 37 . 37 R. S. Sutton. âDyna, an integrated ar- chitecture for learning, planning, and re- actingâ.In: SIGART Bull. 2.4 (1991), p. 160â163 Dyna-Q. The Dyna-Q algorithm is a model-based reinforcement learning algo- rithm that improves the learning efficiency of Q-learning by using the learned model to generate synthetic data 38 . The algorithm is based on the idea that in ad- 38 The term Dyna-Q derives from the fact that the algorithm combines Q-learning with model-based acceleration.The term Dyna more generally refers to the idea of using a learned model to gener- ate synthetic data for model-free learn- ing. dition to updating the Q-function using real-world data, we can also update the Q-function using synthetic data generated by the learned model. At a high-level, the algorithm alternates between three main steps. First, it performs standard Q-learning updates, using real experience collected from interactions with the environment to update the Q-function. Second, it carries out a model learning step, in which the dynamics model is updated based on the same real-world data. Finally, the algorithm executes a model-based acceleration step, during which the learned model is used to generate synthetic experience that is then leveraged to perform additional Q-function updates. A detailed description of this algorithm is provided in Algorithm 18.8. Algorithm 18.8: Dyna-Q Data: Model, p Ξ (x t+1 , r t | x t , u t ), number of model-based acceleration steps, n Result: Updated model parameters, Ξ, action value function Q(x, u)â Q â (x, u) for each episode do Initialize x 0 . for each step t do Select action u t = Ï(x t ). Observe reward r t and next state x t+1 . Q(x t , u t )â Q(x t , u t ) + α ( r t + Îł max u âČ Q(x t+1 , u âČ )â Q(x t , u t ) ) Update model parameters, Ξ, using sample (x t , u t , r t , x t+1 ). for i = 1, . . . , n do Sample x t , u t from real-world data. Generate synthetic data: ( Ëx t+1 , Ë r t )⌠p Ξ (x t+1 , r t | x t , u t ). Q(x t , u t )â Q(x t , u t ) + α ( Ë r t + Îł max u âČ Q( Ëx t+1 , u âČ )â Q(x t , u t ) ) return Ξ, Q(x, u)â Q â (x, u) The Dyna-Q algorithm is a simple yet powerful method that demonstrates the potential of model-based reinforcement learning to improve the learning effi- ciency of model-free algorithms. 18.6.3 Limitations of Model-based Reinforcement Learning Model-based methods are an extremely active and promising area of research in reinforcement learning, but they are also subject to several limitations. First, reinforcement learning 487 model learning entails optimizing the parameters of the model to minimize pre- diction error. However, this objective does not necessarily align with the objective of the agent, which is to maximize the expected cumulative reward, and this discrepancy can lead to suboptimal policies. Second, model-based methods are sensitive to model errors, which can cause the agent to learn suboptimal poli- cies or exploit the model errors to achieve high rewards, potentially leading to catastrophic failures. Finally, learning an accurate model of the environment is a challenging task, especially in complex environments with high-dimensional state and action spaces. 18.7 Summary In this chapter, we provided a comprehensive overview of the field of reinforce- ment learning. Rather than presenting an exhaustive list of algorithms, we fo- cused on a conceptual understanding of the key ideas and principles that under- lie reinforcement learning. In particular, we discussed how Monte Carlo methods and temporal-difference learning represent two foundational paradigms in rein- forcement learning, and how we can use these methods to estimate value func- tions and learn optimal policies. Most importantly, we highlighted how these methods, together with dynamic programming, define a full spectrum of pos- sible approaches to the problem of learning from interaction. Finally, we also discussed concrete examples of the main algorithmic families in reinforcement learning, including model-free and model-based methods, and highlighted the key ideas behind some of the most popular algorithms within these categories. To learn more. For readers interested in a deeper exploration of the topics cov- ered in this chapter, several authoritative resources are available. The definitive reference in the field is the textbook by Sutton and Barto [143], which provides a comprehensive and accessible introduction to the core principles of reinforce- ment learning, including Monte Carlo methods, temporal-difference learning, and function approximation. For a more rigorous treatment of the subject that emphasizes the connections between reinforcement learning, dynamic program- ming, and optimal control, we refer the reader to Bertsekas [14]. 18.8 Exercises The starter code for the exercises provided below is available online through GitHub. To get started, download the code by running in a terminal window: git clone https://github.com/StanfordASL/pora-exercises.git We denote Problems requiring hand-written solutions and coding in Python with and, respectively. 488 principles of robot autonomy Problem 1: Q-learning Widget Sales You are the owner of Widget Co., a shop in the business of buying widgets whole- sale and selling them to consumers at a markup. The shop is able to store between 0 and 5 widgets at a time, and we denote the number of widgets held in the shop on day t as s t . Every day, you choose how many widgets to order from your supplier. You can order either zero widgets, a âhalf orderâ of 2 widgets, or a âfull orderâ of 4 widgets. We write the number of widgets ordered to arrive on day t as a t . A random number of customers (following an unknown distribution, though this distribution may be assumed to be consistent across all days) come to Widget Co. every day; each customer buys a widget if there are any available. We write the demand on day t as d t , and assume d t †5. At the end of each day, you record a net profit r t for that day. In this exercise, we will explore using Q-learning to help model returns and optimize the shopâs performance. In the notebook for this exercise, ch18/exercises/widget _ sales.ipynb, complete the following: 1. We have a datasetD =(s t , a t , r t ) T t=1 containing records for each day t of the last three years of the shopâs operation. In the provided notebook, fill in the function q _ learning to implement a Q-learning algorithm to learn tabulated Q-values from this dataset. 2. Suppose you find that the dynamics of the number of widgets in the shop each day are described by: x t+1 = f (x t , u t , d t ) : = ïŁ± ïŁŽ ïŁŽ ïŁČ ïŁŽ ïŁŽ ïŁł 0,x t + u t â d t < 0 5,x t + u t â d t > 5 x t + u t â d t , otherwise , and the daily net profit is: R(x t , u t , d t ) = c sell min(x t + u t , d t )â c rent â c storage x t â g order (u t ), where c sell = 1.2 is the price you set for each widget, c rent = 1 is the fixed rent on your shop, c storage = 0.05 is the cost for storing each widget overnight, and g order (u t ) = â u t is the cost of ordering widgets from your supplier. The quantity min(x t + u t , d t ) is the âsatisfied demandâ on day t. Letâs also suppose that after a few weeks of sales, you determine that the daily demand distribution for your widgets seems to be: d t = ïŁ± ïŁŽ ïŁŽ ïŁŽ ïŁŽ ïŁŽ ïŁŽ ïŁŽ ïŁŽ ïŁČ ïŁŽ ïŁŽ ïŁŽ ïŁŽ ïŁŽ ïŁŽ ïŁŽ ïŁŽ ïŁł 0, with probability 0.1 1, with probability 0.3 2, with probability 0.3 3, with probability 0.2 4, with probability 0.1 . reinforcement learning 489 In the function action _ value _ iteration, implement value iteration to learn tabulated Q-values from the model described above. Specifically, use the fol- lowing update equation: Q k+1 (x, u) = E d⌠p(d t ) R(x, u, d) + Îł max u âČ Q k ( f (x, u, d), u âČ ) , which is a slight variation of Equation (18.15) for learning Q-values adapted for this problemâs model (see also Equation (18.8)). Compare the Q-values from Q-learning compared to those from value iteration. What do you notice about the learned Q-values compared to those from value iteration? Why do you think this occurs? 3. Finally, compute an optimal policy Ï â QL (x t ) based on the Q-learning approach from the first part, and another optimal policy Ï â VI (x t ) based on the value iteration part. Run the provided code to simulate each one over five years, and compute the cumulative profit â t k=0 r k for each day t and for each optimal policy. Compare the cumulative profits over time. What do you notice about the difference between the two cumulative profit trends? Why do you think this occurs? Problem 2: Cart-pole Balancing via Model-free Reinforcement Learning In this problem, we will return to the classic âcart-poleâ balancing control prob- lem where our goal is to design a controller to balance an inverted pendulum upright on a cart. We have already explored this problem in the context of model- based optimal control (specifically LQR control) in a Chapter 3 exercise, but in this exercise we will approach the problem through model-free reinforcement learning. To summarize the environment setup, the agent observes the state of the envi- ronment as s : = (x, Ì x, Ξ, Ì Îž) â R 4 , where x â R denotes the horizontal position of the cart and Ξ â R denotes the angle of the pendulum from the upright position. At each instant, the agent chooses an action u t â 0, 1 indicating whether to push the cart to the left u t = 0 or to the right u t = 1 39 . 39 Note this is slightly different from the exercise in Chapter 3 where the control was the horizontal force on the cart. This exercise is split up into several parts, and the starter code can be found in the notebook ch18/exercises/cartpole _ balance.ipynb. 1. First, you will implement a deep Q-learning algorithm with experience replay, originally introduced in âPlaying Atari with Deep Reinforcement Learning" 40 . 40 V. Mnih et al. âPlaying Atari with Deep Reinforcement Learningâ.In:ArXiv abs/1312.5602 (2013). url: https : / / api . semanticscholar . org / CorpusID : 15238391 You can find the code for this part in q _ learning.py: (a) Implement the function QLearning.build _ network to create a model for the Q-function that takes as input a state vector and outputs a vector of Q-values for that state and each action. (b) Implement the function QLearning.policy _ train that will be used to com- pute actions during the training process. Implement an Δ-greedy approach that samples a random action with probability Δ to ensure exploration. 490 principles of robot autonomy (c) Implement the functions QLearning.compute _ target and QLearning.tr ain to sample episodes and train the model using experience replay, see Algorithm 18.9 for the training algorithm. (d) Use the provided code to start training with your choice of hyperparame- ters. Algorithm 18.9: Deep Q-learning with Experience Replay Data: Initial action-value model, Q Ï , parameterized by Ï, number of episodes M Result: Improved action-value model Q Ï D â / * Initialize replay buffer * / for episodeâ 1 to M do Initialize x 0 . for each step t of episode do if rand() < Δ then u t â random_sample() else u t â max u Q Ï (x t , u) Execute u t , observe reward r t and next state x t+1 . D.add((x t , u t , r t , x t+1 )) Sample random minibatch of transitions (x, u, r, x âČ ) fromD Lâ 0 for sample (x, u, r, x âČ ) in minibatch do if x is terminal then yâ r else yâ r + Îł max u âČ Q Ï (x âČ , u âČ ) Lâ L + (yâ Q Ï (x, u)) 2 Update model parameters, Ï, using gradient descent on L. return Q Ï 2. Next, you will implement the REINFORCE algorithm introduced in Section 18.5.2. Specifically, in this exercise you will implement three variations of the REIN- FORCE algorithm with slightly different definitions of the policy gradient, one of which is the standard version from Algorithm 18.6. You can find the code for this part in reinforce.py: (a) Implement the function Reinforce.build _ network to create a model for the policy that takes as input a state vector and outputs a vector of action probabilities. (b) Implement the function Reinforce.policy _ train that will be used to com- pute actions during the training process, as well as the log Ï Îž (u t | x t ) value for the chosen action. reinforcement learning 491 (c) Implement the function Reinforce.train to sample episodes and train the model using the outline in Algorithm 18.6. Here, you will implement options for three different policy gradient definitions (i.e. â Ξ V(Ξ)). First, implement the standard REINFORCE algorithm where the objective gradi- entâ Ξ V(Ξ) is estimated using: â Ξ V(Ξ)â Tâ1 â t=0 â Ξ log Ï Îž (u t | x t )R(Ï), based on an episode sampled from the environment and R(Ï) = â Tâ1 t=0 Îł t r t is the total discounted reward of the episode. Next, implement the policy gradient with a âcausality trickâ. This trick avoids reinforcing actions taken later in the episode based on rewards ob- tained early in the episode, since those later actions would have no causality in receiving earlier rewards. Specifically, with this trick you will compute the gradient estimate as: â Ξ V(Ξ)â Tâ1 â t=0 â Ξ log Ï Îž (u t | x t )R t (Ï), where the term R t = â Tâ1 t âČ =t Îł t âČ ât r t âČ is the discounted reward-to-go from time t. Finally, you will implement a policy gradient that uses the âcausality trickâ with a âbaselineâ. The baseline helps to reduce the variance of the pol- icy gradients by âcenteringâ their returns. Specifically, compute the policy gradient as: â Ξ V(Ξ)â Tâ1 â t=0 â Ξ log Ï Îž (u t | x t )(R t (Ï)â b(Ï)), where the baseline b(Ï) is the average of the R t (Ï) reward-to-go values over the episode. You can additionally scale the baseline by the inverse of the standard deviation of the reward-to-go values. (d) Use the provided code to start training with your choice of hyperparame- ters. What differences do you observe between the deep Q-learning algo- rithm and the REINFORCE algorithm in terms of performance during and after training? Which method is more sample efficient, and why? How do the different versions of the policy gradient compare within the REIN- FORCE method? Problem 3: Advantage Actor Critic As discussed in Section 18.5.2, actor-critic methods are a popular variance reduc- tion technique for policy optimization. These methods use a value function as a baseline (the âcriticâ) and the learned policy is the âactorâ. In this problem, you 492 principles of robot autonomy will implement key parts of the Advantage Actor-Critic (A2C) algorithm, which we described in Algorithm 18.7. In the file ch18/exercises/advantage _ actor _ critic.ipynb, complete the following tasks: 1. Using the model components already provided, write the code that imple- ments the complete model, consisting of a couple linear layers to transform the input before being passed to heads for the policy and the value function (i.e. for the actor and critic). For the actor, the model outputs mean and stan- dard deviation parameters for a multivariate normal distribution. 2. For this implementation of A2C, we will use a Monte Carlo estimate to produce value targets for the critic, denoted by y t in Algorithm 18.7. To compute the Monte Carlo value target, implement the function to compute the discounted returns: G t = Tâ1 â k=t Îł kât r k , for an episode. What is the advantage of using Monte Carlo estimates over temporal-difference estimates in terms of the bias-variance tradeoff? 3. Then, implement the function to compute the training loss for an episode: J = Tâ1 â t=0 â Ξ log Ï Îž (u t | x t )A Ï Ï (x t , u t ) + (A Ï Ï (x t , u t )) 2 , where A Ï Ï (x t , u t ) = G t â V Ï Ï (x t ) is the advantage and where the first term is the actor loss and the second (quadratic) term is the critic loss. Note that we are using JAX to compute gradients of the training loss with respect to the model parameters. When computing the training loss for the actor component, its important to use jax.lax.stop _ gradient to stop JAX from backpropagating gradients in this term. Why is this important? 4. Finally, run the provided code to train the model for a toy lunar lander envi- ronment. reinforcement learning 493 References [10]Y. Bai et al. âTraining a Helpful and Harmless Assistant with Reinforce- ment Learning from Human Feedbackâ. In: (2022). url: https://arxiv. org/abs/2204.05862. [14]D. Bertsekas. Reinforcement learning and optimal control. Athena Scientific, 2019. [81]S. Levine et al. âEnd-to-End Training of Deep Visuomotor Policiesâ. In: Journal of Machine Learning Research 17.39 (2016), p. 1â40. [94]V. Mnih et al. âAsynchronous Methods for Deep Reinforcement Learn- ingâ. In: Proceedings of The 33rd International Conference on Machine Learn- ing. 2016, p. 1928â1937. [95]V. Mnih et al. âPlaying Atari with Deep Reinforcement Learningâ. In: ArXiv abs/1312.5602 (2013). url: https://api.semanticscholar.org/ CorpusID:15238391. [100]K. P. Murphy. Probabilistic Machine Learning: An introduction. MIT Press, 2022. [136]D. Silver et al. âMastering the game of Go with deep neural networks and tree searchâ. In: Nature 529.7587 (2016), p. 484â489. [143]R. Sutton and A. Barto. Reinforcement learning: An introduction. MIT Press, 2018. [144]R. S. Sutton. âDyna, an integrated architecture for learning, planning, and reactingâ. In: SIGART Bull. 2.4 (1991), p. 160â163. [158]C. J. C. H. Watkins and P. Dayan. âQ-learningâ. In: Machine Learning 8.3 (1992), p. 279â292. [159]R. J. Williams. âSimple statistical gradient-following algorithms for con- nectionist reinforcement learningâ. In: Machine Learning 8.3 (1992), p. 229â 256. 19 Imitation Learning In Chapter 16, we introduced a strategy for autonomous robot decision making that requires a very manual and sometimes intractable process of specifying de- sired actions from every possible state. Then, in Chapter 17 and Chapter 18, we formulated the sequential decision making problem as an optimization prob- lem where we must specify a cost or reward function that we want the robot to minimize or maximize. This optimization-based approach is more general and scalable, but it still requires us to figure out how to appropriately embed our preferences into the form of a mathematical function. Reward design can be very challenging in practice, and by the nature of optimization-based approaches, the cost or reward function can be inadvertently exploited in undesirable ways. Additionally, in the reinforcement learning context, we require continuous and exploratory interactions with the environment that could be costly or unsafe 1 as 1 For example, some robots operate in close collaboration with humans or in other safety-critical environments where the risk of exploring sub-optimal actions is significant. well as sophisticated learning algorithms that are able to learn from experience. In practice, it can sometimes be easier, more efficient, or safer for human ex- perts to demonstrate the desired task or behavior than it is to precisely program it, try to encode it in a cost function, or let the robot freely interact with the environment. The goal of imitation learning in the context of robotics is to lever- age a limited set of expert demonstrations to accelerate or completely train a robot to autonomously perform a desired behavior. In this chapter, we begin in Section 19.1 by introducing the concept of imitation learning in the context of robotics, provide a formal problem formulation, and discuss key design con- siderations. We then present a canonical imitation learning approach known as Behavior Cloning in Section 19.2, which aims to directly learn a policy from expert demonstrations. Lastly, in Section 19.3, we introduce Inverse Reinforcement Learn- ing, an alternative approach to imitation learning that learns a reward function from expert demonstrations. 19.1 Imitation Learning in Robotics Imitation learning is a class of methods that enable skills to be transferred from an expert to a learner. In the context of robotics, the expert is typically a human operator or a pre-existing control policy, and the learner is the robot that aims to This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 496 principles of robot autonomy mimic the expertâs behavior. While the literature on imitation learning is vast, in this section, we focus on core design decisions and concepts essential for under- standing and applying imitation learning to robotic systems. Specifically, when designing an imitation learning system, several key aspects must be considered: Is imitation learning the right approach? Imitation learning might not always be the most suitable method for learning a task. For example, reinforcement learning might be a more effective approach if it is inexpensive for us to obtain samples from the environment 2 . Moreover, if the expertâs behavior is suboptimal or incon- 2 Such as if we have a good simulator for the task because it could be very safe and cheap to collect data. sistent, imitation learning may not yield the desired performance. Therefore, it is crucial to evaluate whether imitation learning is the most appropriate method for the given task. What should we learn to imitate? Expert demonstrations often contain a substan- tial amount of information that is irrelevant to the task at hand. For example, not all sensor measurements or control signals observed in a demonstration are necessary for successful task execution. Consequently, a central challenge is to identify and extract the aspects of the expertâs behavior that are truly relevant and should be imitated by the learner. Who is the expert? The choice of expert can significantly impact the quality of the learned behavior. In many cases, the expert is a human operator who demon- strates the task. However, the expert could also be a pre-existing control policy, a set of historical data, or a mixture of multiple experts. Understanding which expert to learn from is crucial for the success of the imitation learning process. How should we represent the policy? The choice of policy representation can greatly influence the learning process. For instance, expert behavior can equivalently be represented at different levels of abstraction, such as low-level motor commands, high-level symbolic actions, or trajectory-level demonstrations. Moreover, differ- ent functional forms of the policy, such as whether the policy is defined as a linear function or a neural network, can impact the expressiveness and generalization capabilities of the learned policy. What learning algorithm is most suitable? The choice of learning algorithm can significantly impact the efficiency and performance of the imitation learning pro- cess. Many algorithms have been proposed for imitation learning, each with its own strengths and limitations. Understanding the characteristics of different algorithms and their suitability for the given task is essential for designing an effective imitation learning system. imitation learning 497 19.1.1 Differences Among Imitation Learning, Supervised Learning, and Rein- forcement Learning Imitation learning is often compared with supervised learning and reinforcement learning, as all three paradigms involve learning from data. While these methods share similarities, they also differ in several key aspects. Supervised learning aims to learn a mapping from input data to output la- bels 3 based on a dataset of input-output pairs. While the imitation learning task 3 For example, from camera image in- puts to object category outputs. of deriving a policy from a dataset of expert demonstrations is closely related to supervised learning, there are several key differences. First, in imitation learning, the solution may have inherent structural properties, such as physical constraints or temporal dependencies 4 , that are not present in standard supervised learning 4 For example, in robot planning and control we often have actuation limits. tasks. Second, in a traditional supervised learning setting, we assume that the source domain, which includes the dataset used for training, and the target do- main, which includes the test data, are the same. In imitation learning, we may not be able to directly transfer the expertâs behavior to the learnerâs environment. For example, the embodiment of the expert may differ from the learner, such as if the expert is a human and the learner is a robot, leading to expert demonstra- tions of actions that are not directly executable by the robot. Imitation learning is also typically exposed to the covariate shift problem, where the distribution of the expertâs data may differ from the distribution of the learnerâs data. Specifically, the learner may encounter situations not represented in the expertâs demonstra- tions, requiring it to generalize beyond the expertâs behavior 5 . Lastly, obtaining 5 Strategies for addressing the issues arising from covariate shift will be dis- cussed in more depth later in this chap- ter. expert demonstrations can be costly or time-consuming, making data collection a significant concern. Imitation learning is also closely related to reinforcement learning, as both paradigms involve learning a policy from data that maximizes a reward. How- ever, reinforcement learning methods typically require a predefined reward func- tion to guide the robotâs behavior. In contrast, imitation learning assumes that the expert directly provides optimal, or at least good, behavior, bypassing the need for a reward function. 19.1.2 Problem Formulation In imitation learning problems, we typically assume that we have access to a dataset, D, of expert demonstrations. The dataset generally consists of a set of trajectories and contexts, and we denote it mathematically as D = (Ï i , s i ) N i=1 where N is the number of samples and Ï i = x i,0 , u i,0 , . . . , x i,T is a trajectory executed by the expert in a given context, s i . The context s i may represent a task description, an environmental configuration, or any other relevant information characterizing the expertâs behavior. Alternatively, the dataset may consist of state-action pairs, where we would writeD =(x i , u i ) N i=1 . Given such a dataset, we can broadly identify two main strategies for repro- ducing the expertâs behavior. A first approach is to directly learn a mapping from contexts to trajectories, or from states to actions, using supervised learning 498 principles of robot autonomy techniques. That is, to learn: Ï(s) = Ïor Ï(x) = u. This approach is commonly referred to as Behavior Cloning (BC) 6 . 6 T. Osa et al. âAn Algorithmic Perspec- tive on Imitation Learningâ. In: (2018). url: https://arxiv.org/abs/1811. 06711 Alternatively, we can use the expert demonstrations to learn a reward function, R(x, u), that implicitly defines the expertâs behavior, and then infer a policy that maximizes this reward: Ï â (x) = arg max Ï V Ï = arg max Ï E Ï⌠p Ï (Ï) " Tâ1 â t=0 Îł t R(x t , u t ) # , where V Ï is the expected sum of future rewards for policy Ï where the expec- tation is over possible trajectories Ï that are distributed according to p(Ï). This approach is known as Inverse Reinforcement Learning (IRL) 7 or Inverse Optimal Con- 7 S. Arora and P. Doshi. âA survey of inverse reinforcement learning:Chal- lenges, methods and progressâ. In: Arti- ficial Intelligence 297 (2021), p. 103500 trol (IOC). Behavior cloning and inverse reinforcement learning are the two primary ap- proaches to imitation learning, each with distinct strengths and limitations. In the following sections, we discuss these approaches in more detail and provide insights into when each method is most appropriate. 19.2 Behavior Cloning Behavior cloning is an approach to imitation learning that focuses on directly learning a mapping from states (or contexts) to actions (or trajectories) without explicitly modeling the reward function. The behavior cloning task can be formu- lated as a supervised learning problem, where the policy Ï is learned by solving a regression problem. We outline the general procedure for behavior cloning in Algorithm 19.1. Algorithm 19.1: Behavior Cloning Collect a dataset,D, of expert demonstrations. Define a model architecture for the policy, Ï Îž . Define a loss function,L. Optimize the loss function,L, with respect to the model parameters, Ξ. return Trained policy, Ï Îž . The first step consists of collecting a dataset D of expert demonstrations, for example from logged data generated by a human operator. Next, a model ar- chitecture for the policy Ï Îž is specified, which may take the form of a neural network, a linear model, or another function class described by parameters Ξ. The choice of model architecture depends on the complexity of the task and the amount of available data, since the model must be expressive enough to capture the expertâs behavior, yet not excessively complex to avoid overfitting. Then, a loss function L is defined to quantify the discrepancy between the actions pre- dicted by the policy and those demonstrated by the expert. Common choices imitation learning 499 include mean squared error, â 1 loss, hinge loss, and KullbackâLeibler divergence. Finally, the policy parameters Ξ are optimized by minimizing the loss functionL over the demonstration dataset. Behavior cloning methods are an attractive approach to learning-based deci- sion making, primarily due to their simplicity, effectiveness, and broad applicabil- ity. However, ensuring the learned policy performs reliably in real-world settings presents significant challenges. One of the primary obstacles to trustworthy de- ployment of policies learned through behavior cloning is the issue of covariate shift. 19.2.1 The Covariate Shift Problem Formally, covariate shift refers to a mismatch between the distribution of data encountered during training and the distribution observed at deployment. In the context of behavior cloning, this issue arises when the learned policy is executed in the environment, causing small prediction errors to accumulate over time. As these errors compound, the learner is increasingly likely to visit states that were rarely or never encountered in the expertâs demonstrations, thereby drifting into poorly represented regions of the state space. As a result, the agent is forced to make decisions in unfamiliar situations, leading to poor performance and poten- tially catastrophic failures. While it is impractical to gather data covering all possible states a robot might encounter, several strategies have been developed to mitigate the impact of co- variate shift. These strategies typically follow an iterative process that alternates between updating the robotâs policy and targeted data collection based on the robotâs current state distribution. In this section, we outline two primary ap- proaches to address covariate shift: confidence-based methods and data aggregation methods. Confidence-Based Methods. In the class of confidence-based methods 8 , the agent 8 S. Chernova and M. Veloso. âInterac- tive policy learning through confidence- based autonomyâ. In: Journal of Artificial Intelligence Research 34.1 (2009), p. 1â25. issn: 1076-9757 is endowed with a mechanism for estimating uncertainty in its predictions. This uncertainty estimate is used to identify situations in which the learned policy is likely to make errors, thereby enabling targeted corrective interventions. A common strategy exploits the uncertainty measure to detect regions of the state space where the agentâs decisions are unreliable, after which additional data is collected in those regions to improve policy performance. In some cases, this data collection is also prompted by expert intervention, where the expert temporarily takes control to correct the agentâs actions. At a high level, methods based on this iterative refinement process seek to empirically align the training data distribution with the state distribution induced by the learned policy, thereby mitigating the effects of covariate shift. A schematic overview of this approach is presented in Algorithm 19.2. 500 principles of robot autonomy Algorithm 19.2: Confidence-based Methods Data: Dataset of expert demonstrations,D, confidence estimation function, c(x), confidence threshold, c 0 Result: Trained policy, Ï Îž Train a policy, Ï Îž , on the dataset,D. while true do Observe the state, x t . Compute the confidence estimate, c(x t ). if c(x t ) < c 0 or expert intervention is necessary then Compute additional demonstration data, (x t , u expert t ). D âDâȘ(x t , u expert t ). Train the policy, Ï Îž , on the updated dataset,D. return Trained policy, Ï Îž . Data Aggregation Methods. Data aggregation methods constitute another major class of approaches within behavior cloning. A prominent example is DAGGER 9 , 9 S. Ross, G. Gordon, and D. Bagnell. âA Reduction of Imitation Learning and Structured Prediction to No-Regret On- line Learningâ.In: Proceedings of the Fourteenth International Conference on Ar- tificial Intelligence and Statistics. 2011, p. 627â635 which mitigates covariate shift by explicitly collecting expert demonstrations un- der the state distribution induced by the learnerâs own policy. As outlined in Algorithm 19.3, DAGGER follows an iterative two-step proce- dure. First, the agent is allowed to interact with the environment, thereby gener- ating states according to its current policy and induced state distribution. Second, these visited states are relabeled with expert actions, and the resulting data is ag- gregated into the training set to refine the policy. Algorithm 19.3: DAGGER Algorithm Data: Initial dataset of expert demonstrations,D, initial policy, Ï 1 Ξ , number of iterations, N Result: Trained policy, Ï N Ξ for i = 1, 2, . . . , N do Collect trajectories, Ï =(x t , u robot t ), using the policy Ï i Ξ . Gather dataset of states visited by the robot and actions given by the expert,D i =(x t , u expert t ). Aggregate the dataset,D âDâȘD i . Train the policy, Ï i+1 Ξ , on the updated dataset,D. return Trained policy, Ï N Ξ . In its simplest form, DAGGER begins by initializing the policy, Ï 1 Ξ , using a set of previously collected expert demonstrations. The robot then interacts with the environment with policy Ï 1 Ξ , collecting trajectories Ï that reflect the state distribu- tion under the current policy. These trajectories are subsequently relabeled using the expertâs actions for the visited states. The relabeled trajectories are used to train an updated policy Ï 2 Ξ , which is then employed to collect additional trajec- tories under the state distribution induced by the updated policy. This process is imitation learning 501 repeated for a fixed number of iterations, resulting in the final trained policy Ï N Ξ . By collecting expert demonstrations under the learnerâs state distribution, DAGGER effectively reduces covariate shift and enhances the performance of the learned policy. The method can be viewed as a form of interactive supervised learning, in which the agent actively gathers data to refine its performance. This iterative process minimizes the amount of expert data required and has proven highly effective across a wide range of tasks. In summary, confidence-based methods and data aggregation techniques both provide solutions for addressing the covariate shift problem in behavior cloning. While there are many variations of these methods, the core principles we out- line in Algorithm 19.2 and Algorithm 19.3 provide a foundational understanding of how to mitigate covariate shift through targeted data collection. However, methods following these principles still suffer from other common limitations of behavior cloning, such as dependence on the quality of expert demonstrations. In Section 19.2.2, we discuss approaches that leverage ideas from behavior cloning to learn from broader, and potentially suboptimal, sets of expert demonstrations. 19.2.2 Reinforcement Learning via Supervised Learning (RvS) Recent work 10 has explored the idea of converting the reinforcement learning 10 S. Emmons et al. âRvS: What is Essen- tial for Offline RL via Supervised Learn- ing?â In: 2021 problem, which we discussed in Chapter 18, into a conditional, filtered, or weighted imitation learning problem. These approaches are motivated by a simple but powerful insight: rather than relying exclusively on optimal demonstrations, one can leverage a much broader collection of demonstrations generated by subopti- mal policies or gathered across diverseâyet relatedâtasks. Methods in this class are often referred to as reinforcement learning via supervised learning (RvS). These approaches typically operate by conditioning the policy on goals or desired re- ward levels, and may additionally incorporate mechanisms for reweighting or filtering demonstrations Filtering or Weighting Demonstrations. One common approach to RvS is to filter or weight the expert demonstrations based on their quality or relevance to the task. For example, one might assign higher weights to expert demonstrations that obtain higher rewards. Revisiting the outline of the behavior cloning algorithm in Algorithm 19.1, this idea can be instantiated by modifying the dataset to retain only high-quality demonstrations, as measured by reward information. A simple instantiation of this approach proceeds as follows. First, expert demonstrations are ranked according to their return 11 : 11 The return of a trajectory is the sum of rewards accumulated along the tra- jectory.When ranking trajectories, it is generally desirable to consider long- term performance rather than immedi- ate rewards. r(Ï) = Tâ1 â t=0 Îł t R(x t , u t ). Then, the original dataset D is filtered to retain only the top k% of trajectories based on their return: Ì D =Ï âD | r(Ï)â„ Ì r. 502 principles of robot autonomy where Ì r denotes the return threshold such that k% of the trajectories inD achieve a return greater than or equal to this value. Finally, the policy Ï Îž is trained using the filtered dataset Ì D. This process represents a simple instance of RvS, in which expert trajectories are filtered according to their return. More sophisticated variants instead oper- ate at the level of individual transitions, rather than filtering entire trajectories. In these cases, the quality of individual actions can be assessed using their ad- vantage 12 or, equivalently, their Q-value, instead of relying solely on immediate 12 Recall from Chapter 18 that the advan- tage of an action is defined as the dif- ference between the action-value func- tion and the value function, A(x t , u t ) = Q(x t , u t )â V(x t ). Intuitively, the advan- tage quantifies how much better an ac- tion is relative to the average action in a given state. rewards. Once action weights have been computed, they can be incorporated into the behavior cloning procedure by modifying the loss function in Algorithm 19.1 as: L(Ξ) = E (x,u)âŒ Ì D [ â log Ï Îž (u| x)A(x, u) ] . This objective can be interpreted as a weighted version of the standard behavior cloning loss, where the log-likelihood of each action is scaled by its correspond- ing advantage. This method, commonly referred to as advantage-weighted behav- ior cloning, has been shown to improve the empirical performance of behavior cloning by upweighting higher-quality actions during training. Goal or Reward Conditioning. Another common approach to RvS is to condition the policy on a goal or reward value. This approach is particularly useful in settings where the expert demonstrations are suboptimal or collected from a dif- ferent task. Consider a dataset of previously collected trajectories,D =Ï i . Each trajectory Ï i might be described using different outcomes 13 , such as the final state 13 In other words, a condition that is veri- fied during or at the end of the trajectory. of the trajectory, the total reward obtained, or a specific state visited during the trajectory. Let Ï denote a specific outcome occurring in a trajectory Ï. The goal of conditioning-based RvS is to learn an outcome-conditioned policy, Ï Îž (u | x, Ï), that optimizes: L(Ξ) = E (x,u,Ï)âŒD [ â log Ï Îž (u| x, Ï) ] . Among the various conditioning strategies, goal-conditioned and state-conditioned RvS are particularly relevant. In these approaches, the policy is conditioned on a desired outcome, typically specified as a target state or goal Ï = x â X that the agent is expected to reach. For instance, in a robotic manipulation task, the policy may be conditioned on achieving a specific end-effector configuration. An- other widely used conditioning strategy is reward-conditioned RvS, in which the policy is conditioned on a target reward value, Ï = â Tâ1 t=0 R(x t , u t ). In this setting, the conditioning variable encodes the desired level of performance, allowing the policy to adapt its behavior accordingly. In both cases, conditioning on outcomes enables the robot to extract meaning- ful information from suboptimal or diverse expert demonstrations, often lead- ing to improved performance in practice. For example, consider two policies, Ï Îž 1 (u | x) and Ï Îž 2 (u | x, Ï), that are trained on the same dataset D. Suppose Ï Îž 1 is trained to imitate expert demonstrations that implicitly optimize a spe- cific reward function. This reward-centric approach restricts Ï Îž 1 to behaviors that imitation learning 503 closely follow the expertâs trajectory distribution. On the other hand, Ï Îž 2 is goal- conditioned and trained to achieve any specified goal state, Ï, independent of the underlying reward function. By explicitly incorporating the goal into its policy, Ï Îž 2 decouples the process of achieving desired outcomes from the reward struc- ture. As a result, Ï Îž 2 is likely to generalize better to novel tasks or unseen goal states, as it learns a flexible mapping from states and goals to actions. In contrast, Ï Îž 1 remains constrained by the expertâs reward-aligned demonstrations, making it less adaptable to scenarios with divergent or ambiguous reward structures. 19.3 Inverse Reinforcement Learning In the previous section, we discussed behavior cloning as a form of imitation learning that directly learns a policy from expert demonstrations. Inverse rein- forcement learning 14 takes an orthogonal approach to imitation learning by at- 14 A. Ng and S. Russell.âAlgorithms for Inverse Reinforcement Learningâ. In: Proceedings of the Seventeenth International Conference on Machine Learning. 2000, p. 663â670 tempting to recover a reward function from a policy, or from demonstrations of a policy. In certain cases, identifying the reward function can offer deeper insights into the taskâs underlying structure, making it potentially more informative than directly learning a policy. Additionally, a policy that is optimal for the expert may not be optimal for the agent if they have different dynamics, morphologies, or capabilities 15 . 15 Learned reward representations can also potentially generalize across differ- ent robot platforms that tackle similar problems. Example 19.3.1 (Inverse Reinforcement Learning vs Behavior Cloning). Consider a scenario where the robotâs objective is to drive across a city as quickly as pos- sible. In the context of imitation learning, we assume the reward function is unknown, but an expert provides example routes to navigate the city. Behavior cloning approaches attempt to replicate the expertâs actions, such as by learning to turn right at a particular intersection. This strategy lacks robustness since it can fail when the robot encounters intersections that the expert never visited. In- verse reinforcement learning approaches offer a more generalizable alternative by focusing on identifying key features of the expertâs trajectories, rather than just mimicking actions. For example, instead of merely copying the expertâs turns, the robot could learn to recognize useful patterns, such as preferring roads with higher speed limits or fewer stop signs. The robot can then develop a policy that takes routes with similar advantageous characteristics, even if they differ from the exact paths the expert took. Formally, the goal of inverse reinforcement learning is to recover a reward function, R : X ĂU â R, from a set of expert demonstrations, D = Ï i , where Ï i = (x 0 , u 0 , . . . , x T ) is an example trajectory. The recovered reward function can then be used to train a policy that is optimal with respect to this reward. In particular, given a parametric representation of the reward function with param- eters w, inverse reinforcement learning seeks a parameter configuration that best explains the observed expert behavior. However, the inverse reinforcement learning problem is inherently ill-posed: multiple reward functions may induce the same optimal policy, making the re- 504 principles of robot autonomy ward unidentifiable from demonstrations alone. To mitigate this ambiguity, a variety of alternative objectives have been proposed in the literature, including maximum-margin formulations that separate optimal and suboptimal policies 16 , 16 N. Ratliff, J. A. Bagnell, and M. Zinke- vich.âMaximum Margin Planningâ. In: Proceedings of the 23rd International Conference on Machine Learning. 2006, p. 729â736 as well as maximum-entropy approaches that prefer the least-committal reward function consistent with the demonstrations 17 . 17 B. D. Ziebart et al. âMaximum En- tropy Inverse Reinforcement Learningâ. In: Proceedings of the Twenty-Third AAAI Conference on Artificial Intelligence. 2008, p. 1433â1438 In practice, most inverse reinforcement learning algorithms follow an iterative optimization procedure involving two coupled steps. First, the reward function parameters w are updated according to an algorithm-specific objective. Second, the policy parameters Ξ are adjusted to maximize the expected return under the current reward estimate. These steps are repeated until convergence. Although individual inverse reinforcement learning methods differ in how these updates are performed, Algorithm 19.4 provides a high-level schematic overview of the inverse reinforcement learning process. Algorithm 19.4: High-level IRL Algorithm Data: Expert demonstrations,D, initialized reward function parameters, w, initialized policy parameters, Ξ Result: Learned reward function parameters, w, learned policy parameters, Ξ while not converged do Update the reward function parameters, w. Update the policy parameters, Ξ, to maximize the current estimate of the reward function. return Optimized reward and policy parameters: w, Ξ. In the following sections, we first introduce the concept of feature expectation and then discuss three popular inverse reinforcement learning methods: appren- ticeship learning, maximum margin planning, and maximum entropy IRL. 19.3.1 Feature Expectation We begin by assuming the existence of a true reward function R â that can be expressed as a linear combination of features: R â (x, u) = w â †Ï(x, u), where Ï : X ĂU â [0, 1] d denotes a vector of feature functions 18 . In Chap- 18 To ensure that rewards are bounded by 1, we assumeâ„w â â„ 2 †1. ter 18, we saw that the value function for a policy Ï is defined as the expected cumulative discounted reward as: V Ï T (x) = E Ï⌠p Ï (Ï) " Tâ1 â t=0 Îł t R(x t , Ï(x t ))| x 0 = x # .(19.1) Substituting the linear reward model R(x, u) = w †Ï(x, u) into the expression above yields: V Ï T (x) = w †Ό(Ï, x),(19.2) imitation learning 505 where: ÎŒ(Ï, x) = E Ï⌠p Ï (Ï) " Tâ1 â t=0 Îł t Ï(x t , Ï(x t ))| x 0 = x # . The quantity ÎŒ(Ï, x) is referred to as the feature expectation of policy Ï 19 . 19 For brevity, we may also denote ÎŒ(Ï, x) simply as ÎŒ(Ï) when the depen- dence on the initial state is clear or omit- ted. An important insight is that, by definition, the optimal expert policy Ï â will always yield a value function greater than or equal to that of any other policy and therefore: V Ï â T (x)â„ V Ï T (x), âxâX , âÏ. Using the feature-expectation formulation of the value function in Equation (19.2), this condition can be equivalently expressed as: w â †Ό(Ï â , x)â„ w â †Ό(Ï, x), âxâX , âÏ.(19.3) In principle, one could attempt to recover the expertâs reward vector w â by find- ing a vector w that satisfies the inequality in Equation (19.3). However, this for- mulation is inherently ambiguous. For example, the trivial choice w = 0 satis- fies the inequality for all policies without conveying any meaningful preference. More generally, multiple reward functions may induce the same optimal policy, a phenomenon known as reward ambiguity. This issue lies at the core of inverse re- inforcement learning 20 , and the algorithms discussed below introduce additional 20 A. Ng and S. Russell.âAlgorithms for Inverse Reinforcement Learningâ. In: Proceedings of the Seventeenth International Conference on Machine Learning. 2000, p. 663â670 structure or optimization criteria to resolve this ambiguity. 19.3.2 Apprenticeship Learning The apprenticeship learning algorithm 21 addresses the problem of reward am- 21 P. Abbeel and A. Ng.âApprentice- ship Learning via Inverse Reinforcement Learningâ. In: Proceedings of the Twenty- First International Conference on Machine Learning. 2004 biguity by finding a policy Ï such that the feature expectation induced by Ï is close to that of the expert policy Ï â . Mathematically, the goal of apprenticeship learning is to find a policy such that â„ÎŒ(Ï, x)â ÎŒ(Ï â , x)â„ 2 †Δ for all x â X , where Δ is a small positive constant. For such a policy Ï, we would have that for any w withâ„wâ„ 2 †1: |V Ï T (x)â V Ï â T (x)| =|w †Ό(Ï, x)â w †Ό(Ï â , x)|, â€â„wâ„ 2 â„ÎŒ(Ï, x)â ÎŒ(Ï â , x)â„ 2 †1· Δ = Δ, (19.4) where the first equality follows from the definition of the value function as a func- tion of the feature expectation in Equation (19.2), the first inequality follows from the fact that|x †y|â€â„xâ„ 2 â„yâ„ 2 for any vectors x and y, and the second inequality follows from the assumption that â„wâ„ 2 †1. This result motivates a practical re- formulation of the inverse reinforcement learning problem: rather than attempt- ing to recover the true reward parameters w â , it suffices to learn a policy whose feature expectations match those of the expert within a small tolerance. Under this condition, the learned policy is guaranteed to achieve performance compa- rable to that of the expert, even when the recovered reward function differs from the true underlying reward. 506 principles of robot autonomy Within this framework, the inverse reinforcement learning problem reduces to finding a policy Ï whose induced feature expectation ÎŒ(Ï) closely matches the one of the expert policy, ÎŒ(Ï â ). A schematic overview of the apprenticeship learning algorithm is provided in Algorithm 19.5. Algorithm 19.5: Apprenticeship Learning Data: Expertâs feature expectations, ÎŒ â = ÎŒ(Ï â ), initial policy, Ï 0 Result: Learned parameters, w, and policy, Ë Ï â iâ 1 while true do Compute ÎŒ (iâ1) = ÎŒ(Ï iâ1 ) (or approximate via Monte Carlo methods). Compute t (i) = max w:â„wâ„ 2 â€1 min jâ0,...,(iâ1) w †Ό â â ÎŒ (j) by solving: (w i , t i )â maximize w,t t, subject to w †Ό â â„ w †Ό (j) + t, âjâ0, . . . , (iâ 1), â„wâ„ 2 †1. (19.5) if t i †Δ then Ë Ï â ââ best feature matching policy fromÏ 0 , . . . , Ï iâ1 Ëw â ââ w i return Ë Ï â , Ëw â Compute an optimal policy, Ï i , for the reward function defined by Ëw. iâ i + 1 At iteration i of Algorithm 19.5, we have already identified a set of policies Ï 0 , Ï 1 , . . . , Ï iâ1 along with their corresponding feature expectations ÎŒ (0) , ÎŒ (1) , . . . , ÎŒ (iâ1) . Within the inner loop, we solve the optimization problem defined in Equation (19.5) to estimate a reward parameter vector w that explains the expertâs behavior. Specifically, using the feature-expectation formulation of the value function in Equation (19.2), the constraint: w †Ό â â„ w †Ό (j) + t, can be rewritten as: V Ï â T (x 0 )â„ V Ï j T (x 0 ) + t, meaning that, under the recovered reward function, the expert outperforms each previously learned policy by a margin of at least t. Given the resulting reward parameters w i , we then compute a new policy Ï i 22 that is optimal with respect 22 For example,using reinforcement learning methods introduced in Chap- ter 18. to this reward. This iterative procedure continues until the margin t i falls below a predefined threshold Δ, indicating that the feature expectations of the learned policy sufficiently match those of the expert. imitation learning 507 19.3.3 Maximum Margin Planning Maximum margin planning (MMP) 23 is a generalization of apprenticeship learn- 23 N. Ratliff, J. A. Bagnell, and M. Zinke- vich.âMaximum Margin Planningâ. In: Proceedings of the 23rd International Conference on Machine Learning. 2006, p. 729â736 ing that aims to find a reward function that maximally separates the expert policy from a set of policies. Specifically, MMP modifies Equation (19.5) from the ap- prenticeship learning algorithm as follows: Ëw â = arg min w,Ο â„wâ„ 2 2 + CΟ, subject to w †Ό â â„ w †Ό (j) + m(Ï â , Ï (j) )â Ο, âjâ0, . . . , (iâ 1), (19.6) where m(Ï, Ï âČ ) is a distance function 24 between two policies Ï and Ï âČ , Ο is a 24 For example, m(Ï, Ï âČ ) could measure the number of states in which Ï and Ï âČ select different actions. slack variable that allows for violations of the margin constraints, and C is a hyperparameter that controls the penalty for such violations. Intuitively, this formulation enforces larger margins for policies that are more dissimilar from the expert policy Ï â . An advantage of the MMP formulation over the apprenticeship learning ap- proach in Equation (19.5) arises when the expert is suboptimal. In such cases, it may be impossible to find a reward vector w that makes the expert policy out- perform all other policies. As a result, the apprenticeship learning optimization problem may yield trivial solutions such as w i = 0 and t i = 0. By contrast, the MMP formulation introduces slack variables that relax the margin constraints, en- abling the computation of a nontrivial and informative reward vector even when the expert demonstrations are imperfect. This added flexibility allows MMP to remain effective in realistic scenarios where expert behavior may be noisy or sub- optimal. 19.3.4 Maximum Entropy Inverse Reinforcement Learning As we described in Section 19.3, the inverse reinforcement learning problem is inherently ill-posed since there are infinitely many reward functions that could explain the expertâs behavior. While maximum margin approaches are highly effective when there is a single reward function that is clearly better than alter- natives, in some cases, optimizing for a distribution of reward functions is more appropriate. Maximum entropy inverse reinforcement learning (MaxEnt IRL) 25 25 B. D. Ziebart et al. âMaximum En- tropy Inverse Reinforcement Learningâ. In: Proceedings of the Twenty-Third AAAI Conference on Artificial Intelligence. 2008, p. 1433â1438 aims to find a distribution over reward functions that explains the expertâs be- havior, that matches the feature expectations of the expert 26 , and has maximum 26 Similar to maximum margin methods. entropy 27 . 27 That is, remaining as non-committal or ârandomâ as possible beyond the im- posed constraints. Let us denote the distribution over trajectories induced by a policy Ï as p Ï (Ï). The feature expectations can be rewritten in terms of this distribution as: ÎŒ(Ï) = E Ï [ f (Ï) ] = Z p Ï (Ï) f (Ï)dÏ, where f (Ï) = â Tâ1 t=0 Îł t Ï(x t , Ï(x t )). Within this framework, MaxEnt IRL seeks a 508 principles of robot autonomy trajectory distribution that maximizes entropy: H( p(Ï)) = Z â p(Ï) log p(Ï)dÏ,(19.7) subject to the constraints: Z p(Ï) f (Ï)dÏ = Z p Ï â (Ï) f (Ï)dÏ, Z p(Ï)dÏ = 1, (19.8) where the first constraint enforces that the feature expectations of the learned behavior match those of the expert policy, while the second ensures that p(Ï) is a valid probability distribution. Among the distributions that satisfy the constraint: Z p(Ï) f (Ï)dÏ = Z p Ï â (Ï) f (Ï)dÏ, the maximum entropy distribution follows the exponential form: p(Ï)â exp(w †f (Ï)). Specifically, we can express the trajectory distribution as a function of w as: p(Ï | w) = 1 Z(w) exp w †f (Ï) ,(19.9) where Z(w) is the partition function given by Z(w) = R exp w †f (Ï) dÏ. However, Equation (19.9) only holds for deterministic environments where the next state is fully determined by the current state and action. In stochastic environments, the trajectory distribution is also influenced by the random envi- ronment dynamics, and in this case we express the distribution over trajectories as: p(Ï | w) = 1 Z(w) exp w †f (Ï) Tâ1 â t=0 p(x t+1 | x t , u t ).(19.10) We can therefore obtain the parameter vector w for the reward function by max- imizing the likelihood of the observed data under the maximum entropy distri- bution defined in Equation (19.10) as: Ëw â = arg max w L MLE = arg max w â ÏâD ln p(Ï | w).(19.11) 19.4 Summary In this chapter, we introduced imitation learning, a paradigm for learning control policies by demonstration. We began by formally defining the imitation learning problem, discussing key design considerations, and highlighting its relationship with, and key differences from, supervised and reinforcement learning. Cen- tral to this formulation, we identified two primary strategies: directly learning a policy and inferring an underlying reward function. imitation learning 509 The first strategy, behavior cloning, was presented as a straightforward ap- proach that treats imitation as a supervised learning problem, mapping expert states to actions. We discussed its primary limitation, the covariate shift prob- lem, where small errors accumulate and lead the robot to unfamiliar states. To address this, we explored interactive methods like DAGGER, which mitigate co- variate shift by collecting new demonstrations in states visited by the learnerâs policy. We also introduced reinforcement learning via supervised learning, an extension that leverages suboptimal or diverse demonstration data by filtering, weighting, or conditioning the policy on outcomes like rewards or goals. The second strategy, inverse reinforcement learning, was introduced as an alternative that seeks to recover the expertâs underlying reward function from demonstrations. We highlighted that this approach can lead to more generaliz- able and robust policies, as it captures the expertâs intent rather than their exact actions. We discussed the core concept of feature expectation matching and ex- plored several prominent inverse reinforcement learning algorithms, including Apprenticeship Learning, Maximum Margin Planning, and Maximum Entropy IRL, each offering a different method to resolve the inherent ambiguity in recov- ering a reward function. To learn more. For a comprehensive survey of imitation learning in robotics, in- cluding behavior cloning and its variants, we refer the reader to Osa et al. [107]. The seminal paper on the DAGGER algorithm is presented by Ross, Gordon, and Bagnell [125]. For an introduction to inverse reinforcement learning, the foun- dational work by Ng and Russell [102] is essential. A modern survey on inverse reinforcement learning methods can be found in Arora and Doshi [6]. Specific influential algorithms, such as Apprenticeship Learning and Maximum Entropy IRL, are detailed in Abbeel and Ng [1] and Ziebart et al. [168], respectively. 510 principles of robot autonomy References [1]P. Abbeel and A. Ng. âApprenticeship Learning via Inverse Reinforce- ment Learningâ. In: Proceedings of the Twenty-First International Conference on Machine Learning. 2004. [6]S. Arora and P. Doshi. âA survey of inverse reinforcement learning: Chal- lenges, methods and progressâ. In: Artificial Intelligence 297 (2021), p. 103500. [26]S. Chernova and M. Veloso. âInteractive policy learning through confidence- based autonomyâ. In: Journal of Artificial Intelligence Research 34.1 (2009), p. 1â25. issn: 1076-9757. [34]S. Emmons et al. âRvS: What is Essential for Offline RL via Supervised Learning?â In: 2021. [102]A. Ng and S. Russell. âAlgorithms for Inverse Reinforcement Learningâ. In: Proceedings of the Seventeenth International Conference on Machine Learn- ing. 2000, p. 663â670. [107]T. Osa et al. âAn Algorithmic Perspective on Imitation Learningâ. In: (2018). url: https://arxiv.org/abs/1811.06711. [118]N. Ratliff, J. A. Bagnell, and M. Zinkevich. âMaximum Margin Planningâ. In: Proceedings of the 23rd International Conference on Machine Learning. 2006, p. 729â736. [125]S. Ross, G. Gordon, and D. Bagnell. âA Reduction of Imitation Learning and Structured Prediction to No-Regret Online Learningâ. In: Proceedings of the Fourteenth International Conference on Artificial Intelligence and Statis- tics. 2011, p. 627â635. [168]B. D. Ziebart et al. âMaximum Entropy Inverse Reinforcement Learningâ. In: Proceedings of the Twenty-Third AAAI Conference on Artificial Intelligence. 2008, p. 1433â1438. Prospects A Journey through the landscape of Robot Autonomy At the outset of this book, we asked a deceptively simple question: What is robot autonomy? We described a space between two extremes, where on one end we have the fantastical general-purpose android of science fiction, and on the other end we have the simple, pre-programmed machine that is capable of only a single, narrowly defined task. In that middle ground lives the modern autonomous robot, a system that must perceive, reason, and act in a world that is complex, dynamic, and uncertain. In this book, we have explored the core principles that underpin the design and operation of modern autonomous robots. Specifically, we anchored our dis- cussion in the See-Think-Act cycle, which provides a structured way to understand the flow of information and decision-making in an autonomous system. By now, that abstraction should feel less like a theoretical construct and more like a tech- nical blueprint. Let us briefly revisit each of these stages and reflect on the key insights that emerged from our exploration: See. We emphasized how the See stage is not merely about the passive acquisi- tion of data, but about the active process of extracting meaningful information from the environment. Much of this discussion appeared in Part I, where we be- gan by examining the fundamental properties and limitations of common sensing modalities, both proprioceptive and exteroceptive, and by establishing the phys- ical and geometric principles that govern their measurements. Building on this foundation, we explored how raw sensor data can be transformed into structured and informative representations. We first surveyed classical approaches, includ- ing filtering techniques, feature detection, and methods for extracting geometric information about the environment. We then turned to modern, learning-based techniques, examining how deep neural network architectures enable end-to-end learning of rich representations directly from raw sensory inputs, including im- ages as well as point-based and voxel-based representations of 3D geometry. Fi- nally, we discussed higher-level visual understanding tasks such as object detec- tion and segmentation, in which algorithms must interpret scenes by identifying object instances and reasoning about their spatial relationships. Taken together, the See phase is not simply about sensing, but about percep- This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 512 principles of robot autonomy tion, where the robot must transform raw sensory signals into the rich, semantic representations that are necessary for it to think and act intelligently. Think. Once the See stage has produced a structured and semantically mean- ingful representation of the world, the Think stage is where that representation is transformed into actionable intent. This is the stage in which the robot rea- sons about where it is with respect to its environment, its goals, and how its actions will shape future outcomes. In Part I, we discussed how this process typically begins with robot localization and mapping, where local sensor mea- surements are synthesized into a coherent global estimate of the robotâs state and a map of the environment. Building on this foundation, Part IV introduced techniques for high-level decision-making. Specifically, we discussed approaches to sequential decision-making, where the robot determines, loosely speaking, what to do, ultimately defining the objectives that guide subsequent stages of plan- ning and control. To address this problem, we explored methods rooted in op- timal control, such as dynamic programming, and extended this perspective to modern learning-based approaches, such as reinforcement learning and imitation learning. Across these frameworks, we emphasized different but complementary strategies for reasoning under uncertainty, balancing model-based formulations with data-driven approximations. Viewed holistically, the Think phase emerges as a structured process that trans- forms information about the environment and abstract goals into concrete plans and intentions that can be executed by the robot. Act. The final stage of the cycle, Act, is where high-level decisions are translated into physical motion. While the Think stage determines what the robot should do, the Act stage determines how those decisions are realized by a dynamical system subject to physical constraints. In Part I, we developed the tools required to bridge this gap. We began with modeling the robot as a dynamical system, introducing the representations and notation needed to describe its motion and constraints. Building on this foundation, we discussed how desired behaviors are converted into feasible trajectories through motion planning and trajectory optimization. Motion planning algorithms produce geometrically feasible, collision-free paths, while trajectory optimization refines these paths into time-parameterized mo- tions that respect dynamics, actuator limits, and task requirements. Execution, however, is never merely the blind following of precomputed motor commands. The real world is inherently uncertain, subject to external disturbances, sensor noise, actuator limitations, and inevitable inaccuracies in the robotâs internal dy- namic models. For this reason, execution must be grounded in the principle of feedback control. Rather than assuming that a planned trajectory will unfold ex- actly as predicted, feedback control continuously compares the robotâs measured state against its desired state and corrects deviations in real time. Our discus- sion spanned a spectrum of closed-loop control strategies, from classical PID to optimization-based methods, such as Linear Quadratic Regulators (LQR) and prospects 513 Model Predictive Control (MPC), which explicitly reason about system dynamics, performance objectives, and constraints. Viewed in the context of the full See-Think-Act cycle, the Act phase closes the loop between intention and reality. It is where plans meet physical embodiment, and where the robotâs ability to adapt and respond to the unpredictable nature of the real world is put to the test. From Modular Pipelines to End-to-End Autonomy. Throughout this book, we have emphasized that the stages of See, Think, and Act are not isolated modules, but deeply interconnected processes with informa- tion flowing asynchronously at different rates, and with multiple feedback loops. At the same time, the field is undergoing a noticeable shift from carefully en- gineered, modular autonomy stacks toward increasingly end-to-end frameworks. In classical architectures, perception, state estimation, planning, and control are designed and tuned as distinct components, each with explicit interfaces and well-defined responsibilities. In contrast, modern data-driven systems often blur or even collapse these boundaries, and can now solve tasks that were once con- sidered beyond the reach of traditional, non-learning-based systems. This shift toward end-to-end learning has been enabled by the availability of large-scale datasets, increasingly realistic simulation environments, and unprece- dented computational resources. End-to-end learning promises greater adapt- ability, reduced manual engineering, and the ability to capture complex couplings that are difficult to model analytically. Yet, this evolution does not make the ma- terial in this book any less relevant, as the principles and techniques of sensing, estimation, planning, and control continue to underpin the design and analysis of these end-to-end systems. For instance, modular components often serve as strong baselines against which end-to-end approaches are compared. Classical algorithms frequently reappear as safety layers, fallback strategies, or constraint- enforcing mechanisms wrapped around learned policies, ensuring that the sys- tem maintains a degree of interpretability, reliability, and safety. Moreover, this data-centric paradigm introduces new system-level challenges. The design of datasets, labeling pipelines, simulation environments, evaluation metrics, and benchmarking protocols becomes even more critical, and often draws heavily on traditional techniques from perception, state estimation, and control. In this sense, the traditional See-Think-Act abstraction continues to provide not only a conceptual framework, but also a powerful lens through which to analyze, debug, and responsibly deploy the increasingly end-to-end systems that are shaping the future of robot autonomy. The Road Ahead As we write this book, we find ourselves at a remarkable inflection point in robotics research, where advances in computation, sensing, machine learning, 514 principles of robot autonomy and large-scale data are rapidly reshaping how autonomous systems are de- signed, developed, and operated. In this final section, we will briefly reflect on some of the most exciting emerging trends in robotics research, and how they are shaping the future of robot autonomy. Foundation Models (FMs). Over the past few years, artificial intelligence has un- dergone a dramatic shift driven by large-scale foundation models. Citing the semi- nal work of Bommasani et al. [18], we define foundation models as: â[Foundation models are] models that are trained on broad data (generally using self-supervision at scale) that can be adapted (e.g., fine-tuned) to a wide range of downstream tasks.â Importantly, the term âfoundation modelâ is not meant to refer to a specific ar- chitecture. Rather, it describes a class of models characterized by scale, broad pretraining, and adaptability. Well-known examples include large language mod- els (LLMs) 28 29 , which underpin systems such as ChatGPT, as well as vision- 28 A. Radford et al. âLanguage Models are Unsupervised Multitask Learnersâ. In: (2019) 29 T. B. Brown et al. âLanguage Models are Few-Shot Learnersâ. In: Advances in Neural Information Processing Systems. 2020, p. 1877â1901 language models (VLMs) 30 , video generation models 31 , and other large-scale 30 A. Radford et al. âLearning Transfer- able Visual Models From Natural Lan- guage Supervisionâ. In: Proceedings of the 38th International Conference on Ma- chine Learning. 2021, p. 8748â8763 31 J. Ho et al. âImagen Video:High Definition Video Generation with Dif- fusion Modelsâ.In:arXiv preprint arXiv:2210.02303 (2022) architectures 32 . Beyond their impact on purely digital applications, foundation 32 A. Rives et al. âBiological structure and function emerge from scaling unsuper- vised learning to 250 million protein se- quencesâ. In: Proceedings of the National Academy of Sciences 118.15 (2021) models are increasingly being integrated into physical robotic systems. Their growing relevance in robotics stems from several key properties. First, foundation models provide broad priors about the world. Trained on large and diverse datasets, they encode a wealth of information about the structure of lan- guage, visual concepts, and physical interactions, enabling improved generaliza- tion in settings with limited task-specific data. Second, foundation models are inherently multimodal. Many modern architec- tures jointly process text, images, video, audio, and structured data. Robotics, by its very nature, is a multimodal domain where robots must integrate information from diverse sensory modalities such as vision, proprioception, tactile sensing, and language-based instructions. Multimodal foundation models offer a unified representational framework in which these heterogeneous data streams can be fused and reasoned over coherently. Third, foundation models provide a powerful and flexible interface for human- robot interaction. Language-conditioned policies enable users to specify goals in natural language rather than low-level commands. This dramatically lowers the barrier between human intent and robotic execution, enabling more intuitive forms of supervision, correction, and collaboration. Within this broader paradigm, several emerging instantiations of foundation models are particularly influential in robotics: Visionâlanguageâaction (VLA) and reasoning models. VLA models 33 34 35 rep- 33 B. Zitkovich et al. âRT-2:Vision- Language-Action Models Transfer Web Knowledge to Robotic Controlâ. In: Pro- ceedings of The 7th Conference on Robot Learning. 2023, p. 2165â2183 34 K. Black et al. â pi0:A Vision-Language-Action Flow Model for General Robot Controlâ. In: (2024) 35 M. J. Kim et al. âOpenVLA: An Open- Source Vision-Language-Action Modelâ. In: Proceedings of The 8th Conference on Robot Learning. 2025, p. 2679â2713 resent an emerging class of architectures that directly couple vision and language representations to action outputs. In contrast to traditional pipelines where per- ception, planning, and control are engineered as separate modules, VLA models attempt to learn end-to-end mappings from multimodal inputs to motor com- mands or high-level actions, essentially learning the See-Think-Act cycle as a sin- gle, integrated process. Typically pretrained on large-scale vision and language prospects 515 data and subsequently fine-tuned on robotic interaction data, these models aim to combine broad semantic understanding with embodied control. A closely related development is the emergence of reasoning models within em- bodied systems 36 37 38 . Beyond mapping observations directly to actions, these 36 W. Huang et al. âInner Monologue: Embodied Reasoning through Planning with Language Modelsâ. In: Proceedings of The 6th Conference on Robot Learning. 2023, p. 1769â1782 37 NVIDIA.âAlpamayo-R1:Bridging Reasoning and Action Prediction for Generalizable Autonomous Driving in the Long Tailâ.In:arXiv preprint arXiv:2511.00088 (2025) 38 M. Zawalski et al. âRobotic Control via Embodied Chain-of-Thought Rea- soningâ. In: Proceedings of The 8th Con- ference on Robot Learning. 2024 architectures allocate explicit computation to intermediate deliberation, gener- ating structured reasoning tracesâoften in natural languageâthat attempt to make causal relationships, counterfactuals, and task constraints explicit before committing to specific actions. Inspired by advances in large language models where inference-time reasoning improves robustness and accuracy, reasoning- enabled robotic systems treat deliberation itself as a tunable resource, where more complex or safety-critical situations can trigger deeper chains of thought, self- reflection, or verification steps. In embodied contexts, however, reasoning cannot remain purely textual. It must be grounded in geometry, dynamics, and physi- cal feasibility, and must remain consistent with the actions ultimately executed. The central challenge, therefore, is not merely to produce articulate explanations, but to ensure alignment between internal rationale and external behavior, so that reasoning becomes a functional component of autonomy rather than a post-hoc narrative. World models. Another especially important instantiation of the foundation model paradigm for robotics is the development of world models. Broadly speak- ing, world models aim to capture how the world evolves over time, including its physical, spatial, and visual dynamics 39 40 . Unlike traditional dynamics models, 39 D.HaandJ.Schmidhuber. âWorld Modelsâ.In:arXiv preprint arXiv:1803.10122 (2018) 40 NVIDIA.âCosmos World Founda- tion Model Platform for Physical AIâ. in: arXiv preprint arXiv:2501.03575 (2025) which operate on carefully designed state representations, modern world mod- els are learned directly from rich, high-dimensional inputs such as images, video, depth, and other multimodal signals. This allows them to model complex aspects of real-world environmentsâsuch as object persistence, scene geometry, contact interactions, and temporal visual changeâthat are difficult to specify analytically but are essential for robust autonomy. This richer predictive interface is particularly valuable in robotics, where data is a fundamental bottleneck. Robot interaction data is expensive to collect, embodiment- specific, and limited in scale. In contrast, visual dataâespecially videoâis abun- dant and diverse. World models provide a pathway for leveraging this disparity by learning general physical and semantic structure from large-scale, largely pas- sive observations. In doing so, they enable a partial decoupling between world understanding and control, allowing robots to acquire broad knowledge about how environments behave without requiring equivalent amounts of task-specific inter- action data. At a high level, world models equip robots with the ability to predict and imag- ine. Given a current situation and, potentially, a proposed action sequence, a world model can forecast how the environment and the robot within it may evolve. This enables planning through the evaluation of imagined futures, where a robot may compare alternative outcomes, select actions that lead toward desir- able states, or optimize behavior by reasoning directly over predicted trajectories. A second major advantage is that world models can support the generation of 516 principles of robot autonomy realistic synthetic data. Because they model visual, spatial, and physical regular- ities, world models can be used to synthesize new scenarios, including rare edge cases or safety-critical situations that are difficult to collect in practice. In this way, world models can augment traditional simulation, providing a data-driven substrate for training and evaluating autonomous systems in a broader range of conditions, while maintaining high visual fidelity. The precise form that such world models should take remains an open re- search question. Some approaches focus on predicting high-dimensional sen- sory observations such as future video frames conditioned on actions 41 . Others 41 Google DeepMind.Genie 3: A New Frontier for World Models. Research blog. 2025. url: https://deepmind.google/ blog/genie- 3- a- new- frontier- for- world-models/ learn compact latent representations of the environment and predict future states within this lower-dimensional space rather than at the raw pixel level 42 . Ad- 42 M. Assran et al. âV-JEPA 2:Self- Supervised Video Models Enable Under- standing, Prediction and Planningâ. In: arXiv preprint arXiv:2506.09985 (2025) ditional work explores structured representations that encode objects, physical interactions, or causal relationships in an effort to improve generalization and data efficiency 43 . 43 F. Locatello et al. âObject-Centric Learning with Slot Attentionâ. In: Ad- vances in Neural Information Processing Systems 33. 2020, p. 11525â11538 Taken together, while the integration of foundation models into robotics remains an active and rapidly evolving area of research, recent developments suggest that they may fundamentally reshape how autonomous systems are designed, developed, and operated. Rather than building task-specific autonomy stacks from scratch, future robots may increasingly rely on large pretrained models as adaptable cores, as well as key enablers across the entire autonomy development pipeline. In this sense, foundation models are not merely new building blocks within the autonomy stack, but are beginning to redefine the abstractions and workflows used to construct it. Simulation and Closing the Gap to Reality. In parallel, progress in simulation has accelerated dramatically, expanding both its fidelity and its role within the auton- omy development pipeline. Advances in physics-based engines, high-fidelity ren- dering, differentiable simulators, and large-scale synthetic data generation now enable training and evaluation in increasingly realistic environments 44 . At the 44 V. Makoviychuk et al. âIsaac Gym: High Performance GPU Based Physics Simulation For Robot Learningâ. In: Pro- ceedings of the Neural Information Process- ing Systems Track on Datasets and Bench- marks. 2021 same time, the emergence of generative simulation has begun to complement traditional simulators by producing realistic, diverse, and often open-ended sce- narios directly from data. Simulation has long been an essential tool in robotics, but it is increasingly becoming a central substrate for large-scale training, evaluation, and red-teaming of autonomous systems. Physical AI Safety. As autonomy stacks incorporate more data-driven compo- nents, the question of safety becomes increasingly critical. Data-driven models can hallucinate, extrapolate poorly outside their training distribution, or produce internally inconsistent reasoning traces. In physical systems, such failures carry real-world consequences. Physical AI safety is therefore becoming a central topic. This includes research on detecting and mitigating failure modes of data-driven models, developing ro- prospects 517 bust training and evaluation protocols, and combining data-driven learning with traditional control-theoretic safety guarantees 45 46 47 48 . At the systems level, 45 R. Sinha et al. âReal-Time Anomaly Detection and Reactive Planning with Large Language Modelsâ. In: Proceed- ings of Robotics: Science and Systems. 2024 46 S. Sagawa et al. âDistributionally Ro- bust Neural Networks for Group Shifts: On the Importance of Regularization for Worst-Case Generalizationâ.In: Pro- ceedings of the International Conference on Learning Representations. 2020 47 J. Hoffman et al. âCyCADA: Cycle- Consistent Adversarial Domain Adapta- tionâ. In: Proceedings of the 35th Inter- national Conference on Machine Learning. 2018, p. 1994â2003 48 A. Taylor et al. âLearning for Safety- Critical Control with Control Barrier Functionsâ. In: Learning for Dynamics and Control. 2020, p. 708â717 emerging frameworks such as NVIDIAâs Halos platform aim to provide end-to- end safety validation pipelines for autonomous systems, integrating simulation, scenario generation, and formal evaluation to stress-test models under diverse and safety-critical conditions 49 . Such efforts highlight the growing recognition 49 NVIDIA. NVIDIA Halos: Autonomous Vehicle Safety. 2024. url: https://w. nvidia.com/en-us/ai-trust-center/ halos/autonomous-vehicles/ that safety must be addressed holistically, spanning model design, data curation, evaluation, and deployment. An Open Frontier One thing is clear: it is an exciting time to be working in robotics. Autonomous systems that were once confined to research laboratories are increasingly de- ployed in real-world environments and embedded within critical infrastructure. From autonomous vehicles and aerial systems to space robotics and medical plat- forms, the impact of robotics is expanding rapidly. With this book, our aim has been twofold. First, to provide the reader with the mathematical and algorithmic foundations that underpin modern robot auton- omy, so that they can understand the core principles and techniques that enable robots to perceive, plan, and act in complex environments. Second, to cultivate a systems-level perspective, so that the reader may understand not only individ- ual algorithms, but how they interconnect and compose into complete autonomy stacks. The future of robotics will not be built by isolated techniques, but by thoughtful integration. It will be shaped by engineers and researchers who un- derstand both theory and systems, both abstraction and embodiment. The tools are evolving rapidly, but the underlying principles will remain enduring guides. The next chapter of robot autonomy will be written by those who are willing to build, and we hope this book has given the reader the foundation to do just that. References [1]P. Abbeel and A. Ng. âApprenticeship Learning via Inverse Reinforce- ment Learningâ. In: Proceedings of the Twenty-First International Conference on Machine Learning. 2004. [2]J. Alammar. The Illustrated Transformer. https://jalammar.github.io/ illustrated-transformer/. Blog post. Accessed: 2026-04-09. June 2018. [3]R. Alur. Algorithms for Decision Making. MIT Press, 2015. [4]A. Amini et al. âDeep evidential regressionâ. In: Advances in neural infor- mation processing systems 33 (2020), p. 14927â14937. [5]A. N. Angelopoulos, S. Bates, et al. âConformal prediction: A gentle in- troductionâ. In: Foundations and trendsÂź in machine learning 16.4 (2023), p. 494â591. [6]S. Arora and P. Doshi. âA survey of inverse reinforcement learning: Chal- lenges, methods and progressâ. In: Artificial Intelligence 297 (2021), p. 103500. [7]U. M. Ascher and R. D. Russell. âReformulation of boundary value prob- lems into âstandardâ formâ. In: SIAM Review 23.2 (1981), p. 238â254. [8]M. Assran et al. âV-JEPA 2: Self-Supervised Video Models Enable Un- derstanding, Prediction and Planningâ. In: arXiv preprint arXiv:2506.09985 (2025). [9]K. J. Aström and R. M. Murray. Feedback Systems. Princeton University Press, 2009. [10]Y. Bai et al. âTraining a Helpful and Harmless Assistant with Reinforce- ment Learning from Human Feedbackâ. In: (2022). url: https://arxiv. org/abs/2204.05862. [11]Y. Bar-Shalom, X. R. Li, and T. Kirubarajan. Estimation with applications to tracking and navigation: theory algorithms and software. John Wiley & Sons, 2001. [12]D. Bertsekas. Dynamic Programming and Optimal Control. Athena Scientific, 2000. [13]D. Bertsekas. Nonlinear Programming. Athena Scientific, 2016. [14]D. Bertsekas. Reinforcement learning and optimal control. Athena Scientific, 2019. This material will be published by Cambridge University Press as Principles of Robot Autonomy by Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone. This pre-publication version is free to view and download for personal use only. Not for re-distribution, re-sale or use in derivative works. © Daniele Gammelli, Joseph Lorenzetti, Katie Luo, Gioele Zardini and Marco Pavone 2026. 520 principles of robot autonomy [15]D. P. Bertsekas. âThe auction algorithm: A distributed relaxation method for the assignment problemâ. In: Annals of operations research 14.1 (1988), p. 105â123. [16]K. Black et al. â pi0: A Vision-Language-Action Flow Model for General Robot Controlâ. In: (2024). [17]S. Blackman and R. Popoli. Design and Analysis of Modern Tracking Systems. Artech House, 1999. [18]R. Bommasani et al. âOn the Opportunities and Risks of Foundation Mod- elsâ. In: arXiv preprint arXiv:2108.07258 (2021). [19]F. Borrelli, A. Bemporad, and M. Morari. Predictive Control for Linear and Hybrid Systems. Cambridge University Press, 2017. [20]G. Bradski. âThe OpenCV Libraryâ. In: Dr. Dobbâs Journal of Software Tools (2000). [21]T. B. Brown et al. âLanguage Models are Few-Shot Learnersâ. In: Advances in Neural Information Processing Systems. 2020, p. 1877â1901. [22]C. Cadena et al. âPast, present, and future of simultaneous localization and mapping: Toward the robust-perception ageâ. In: IEEE Transactions on robotics 32.6 (2017), p. 1309â1332. [23]Nicolas Carion et al. âEnd-to-End Object Detection with Transformersâ. In: Computer Vision â ECCV 2020. Springer International Publishing, 2020, p. 213â229. [24]L. Carlone et al., eds. SLAM Handbook. From Localization and Mapping to Spatial Intelligence. Cambridge University Press, 2026. [25]D. S. Chaplot et al. âLearning to explore using active neural slamâ. In: arXiv preprint arXiv:2004.05155 (2020). [26]S. Chernova and M. Veloso. âInteractive policy learning through confidence- based autonomyâ. In: Journal of Artificial Intelligence Research 34.1 (2009), p. 1â25. issn: 1076-9757. [27]F. Dellaert. âFactor graphs and GTSAM: A hands-on introductionâ. In: Georgia Institute of Technology, Tech. Rep 2.4 (2012). [28]F. Dellaert. âFactor graphs: Exploiting structure in roboticsâ. In: Annual Review of Control, Robotics, and Autonomous Systems 4.1 (2021), p. 141â166. [29]F. Dellaert and M. Kaess. âSquare root SAM: Simultaneous localization and mapping via square root information smoothingâ. In: The International Journal of Robotics Research 25.12 (2006), p. 1181â1203. [30]F. Dellaert et al. âMonte carlo localization for mobile robotsâ. In: Proceed- ings 1999 IEEE international conference on robotics and automation (Cat. No. 99CH36288C). Vol. 2. IEEE. 1999, p. 1322â1328. REFERENCES 521 [31]D. DeTone, T. Malisiewicz, and A. Rabinovich. âSuperpoint: Self-supervised interest point detection and descriptionâ. In: Proceedings of the IEEE con- ference on computer vision and pattern recognition workshops. 2018, p. 224â 236. [32]A. Dosovitskiy et al. âAn Image is Worth 16x16 Words: Transformers for Image Recognition at Scaleâ. In: International Conference on Learning Repre- sentations. 2021. [33]G. Dudek and M. Jenkin. âInertial Sensors, GPS, and Odometryâ. In: Springer Handbook of Robotics. Springer, 2008, p. 477â490. [34]S. Emmons et al. âRvS: What is Essential for Offline RL via Supervised Learning?â In: 2021. [35]M. A. Fischler and R. C. Bolles. âRandom sample consensus: a paradigm for model fitting with applications to image analysis and automated car- tographyâ. In: Commun. ACM 24.6 (1981), p. 381â395. [36]C. Forster et al. âOn-manifold preintegration for real-time visualâinertial odometryâ. In: IEEE Transactions on Robotics 33.1 (2016), p. 1â21. [37]D. A. Forsyth and J. Ponce. Computer Vision: A Modern Approach. Prentice Hall, 2011. [38]A. Fusiello, E. Trucco, and A. Verri. âA compact algorithm for rectification of stereo pairsâ. In: Machine Vision and Applications 12.1 (2000), p. 16â22. [39]A. Gelb et al. Applied optimal estimation. MIT press, 1974. [40]R. Girshick. âFast R-CNNâ. In: 2015 IEEE International Conference on Com- puter Vision (ICCV). 2015, p. 1440â1448. doi: 10.1109/ICCV.2015.169. [41]I. Goodfellow, Y. Bengio, and A. Courville. Deep Learning. http://w. deeplearningbook.org. MIT Press, 2016. [42]Google DeepMind. Genie 3: A New Frontier for World Models. Research blog. 2025. url: https://deepmind.google/blog/genie-3-a-new-frontier- for-world-models/. [43]C. Guo et al. âOn calibration of modern neural networksâ. In: International conference on machine learning. PMLR. 2017, p. 1321â1330. [44]F. Gustafsson. Statistical Sensor Fusion. Studentlitteratur, 2013, p. 554. [45]D. Ha and J. Schmidhuber. âWorld Modelsâ. In: arXiv preprint arXiv:1803.10122 (2018). [46]D. Harel. âStatecharts: A visual formalism for complex systemsâ. In: Sci- ence of Computer Programming 8.3 (1987), p. 231â274. [47]C. Harris and M. Stephens. âA combined corner and edge detectorâ. In: 4th Alvey Vision Conference. 1988. [48]R. Hartley and A. Zisserman. âCamera Modelsâ. In: Multiple View Geome- try in Computer Vision. Academic Press, 2002. 522 principles of robot autonomy [49]K. Hauser. âMotion and Path Planningâ. In: Encyclopedia of Robotics. Springer, 2020, p. 1â11. [50]K. He et al. âMask R-CNNâ. In: Proceedings of the IEEE International Con- ference on Computer Vision. 2017, p. 2961â2969. [51]J. Hertling. âNumerical Methods for Two-Point Boundary Value Problems (Herbert B. Keller)â. In: SIAM Review 12.2 (1970), p. 313â315. [52]J. Ho et al. âImagen Video: High Definition Video Generation with Diffu- sion Modelsâ. In: arXiv preprint arXiv:2210.02303 (2022). [53]J. Hoffman et al. âCyCADA: Cycle-Consistent Adversarial Domain Adap- tationâ. In: Proceedings of the 35th International Conference on Machine Learn- ing. 2018, p. 1994â2003. [54]J. P. How. Lecture Notes for Principles of Optimal Control. 2008. [55]W. Huang et al. âInner Monologue: Embodied Reasoning through Plan- ning with Language Modelsâ. In: Proceedings of The 6th Conference on Robot Learning. 2023, p. 1769â1782. [56]L. Janson et al. âDeterministic sampling-based motion planning: Optimal- ity, complexity, and performanceâ. In: Int. Journal of Robotics Research 37.1 (2018), p. 46â61. [57]L. Janson et al. âFast Marching Tree: A Fast Marching Sampling-Based Method for Optimal Motion Planning in Many Dimensionsâ. In: Int. Jour- nal of Robotics Research 34.7 (2015), p. 883â921. [58]L. Joseph. Robot Operating System (ROS) for Absolute Beginners: Robotics Programming Made Easy. Apress, 2018. [59]S. J. Julier and J. K. Uhlmann. âA non-divergent estimation algorithm in the presence of unknown correlationsâ. In: Proceedings of the 1997 American Control Conference (Cat. No. 97CH36041). Vol. 4. IEEE. 1997, p. 2369â2373. [60]S. J. Julier and J. K. Uhlmann. âNew extension of the Kalman filter to nonlinear systemsâ. In: Signal processing, sensor fusion, and target recognition VI. Vol. 3068. Spie. 1997, p. 182â193. [61]L. Kaelbling et al. 6.01SC: Introduction to Electrical Engineering and Computer Science I. MIT OpenCourseWare. 2011. [62]M. Kaess, A. Ranganathan, and F. Dellaert. âiSAM: Incremental smooth- ing and mappingâ. In: IEEE Transactions on Robotics 24.6 (2008), p. 1365â 1378. [63]M. Kaess et al. âiSAM2: Incremental smoothing and mapping using the Bayes treeâ. In: The International Journal of Robotics Research 31.2 (2012), p. 216â235. [64]Z. Kang et al. âVoxel-Based Extraction and Classification of 3-D Pole-Like Objects From Mobile LiDAR Point Cloud Dataâ. In: IEEE Journal of Se- lected Topics in Applied Earth Observations and Remote Sensing 11 (Nov. 2018), p. 4287â4298. doi: 10.1109/JSTARS.2018.2869801. REFERENCES 523 [65]S. Karaman and E. Frazzoli. âSampling-based Algorithms for Optimal Motion Planningâ. In: Int. Journal of Robotics Research 30.7 (2011), p. 846â 894. [66]L. E. Kavraki et al. âProbabilistic roadmaps for path planning in high- dimensional configuration spacesâ. In: IEEE Transactions on Robotics and Automation 12.4 (1996), p. 566â580. [67]M. Kelly. âAn Introduction to Trajectory Optimization: How to Do Your Own Direct Collocationâ. In: SIAM Review 59.4 (2017), p. 849â904. [68]O. Khatib. âReal-Time Obstacle Avoidance for Manipulators and Mobile Robotsâ. In: The International Journal of Robotics Research 5.1 (1986), p. 90â 98. [69]M. J. Kim et al. âOpenVLA: An Open-Source Vision-Language-Action Modelâ. In: Proceedings of The 8th Conference on Robot Learning. 2025, p. 2679â 2713. [70]D. E. Kirk. Optimal Control Theory: An Introduction. Dover Publications, 2004. [71]G. Klein and D. Murray. âParallel tracking and mapping for small AR workspacesâ. In: 2007 6th IEEE and ACM international symposium on mixed and augmented reality. IEEE. 2007, p. 225â234. [72]H. W. Kuhn. âThe Hungarian method for the assignment problemâ. In: Naval research logistics quarterly 2.1-2 (1955), p. 83â97. [73]R. KĂŒmmerle et al. âg 2 o: A general framework for graph optimizationâ. In: 2011 IEEE international conference on robotics and automation. IEEE. 2011, p. 3607â3613. [74]B. Lakshminarayanan, A. Pritzel, and C. Blundell. âSimple and scalable predictive uncertainty estimation using deep ensemblesâ. In: Advances in neural information processing systems 30 (2017). [75]A. H. Lang et al. âPointpillars: Fast encoders for object detection from point cloudsâ. In: Proceedings of the IEEE/CVF conference on computer vision and pattern recognition. 2019, p. 12697â12705. [76]J. C. Latombe. Robot Motion Planning. USA: Kluwer Academic Publishers, 1991. [77]S. M. LaValle. Planning Algorithms. Cambridge, U.K.: Cambridge Univer- sity Press, 2006. [78]S. M. LaValle. Rapidly-Exploring Random Trees: A New Tool for Path Planning. 1998. [79]J. J. Leonard and H. F. Durrant-Whyte. âSimultaneous map building and localization for an autonomous mobile robot.â In: IROS. Vol. 3. 1991, p. 1442â1447. 524 principles of robot autonomy [80]J. Levine. Analysis and Control of Nonlinear Systems: A Flatness-based Ap- proach. Springer, 2009. [81]S. Levine et al. âEnd-to-End Training of Deep Visuomotor Policiesâ. In: Journal of Machine Learning Research 17.39 (2016), p. 1â40. [82]M. Liggins I, D. Hall, and J. Llinas. Handbook of multisensor data fusion: theory and practice. CRC press, 2017. [83]M. Liggins I, D. Hall, and J. Llinas. Handbook of multisensor data fusion: theory and practice. CRC press, 2017. [84]F. Locatello et al. âObject-Centric Learning with Slot Attentionâ. In: Ad- vances in Neural Information Processing Systems 33. 2020, p. 11525â11538. [85]C. Loop and Z. Zhang. âComputing rectifying homographies for stereo visionâ. In: IEEE Computer Society Conference on Computer Vision and Pattern Recognition. Vol. 1. 1999, p. 125â131. [86]D. G. Lowe. âDistinctive Image Features from Scale-Invariant Keypointsâ. In: International Journal of Computer Vision 60.2 (Nov. 2004), p. 91â110. issn: 1573-1405. doi: 10.1023/B:VISI.0000029664.99615.94. [87]D. G. Lowe. âObject recognition from local scale-invariant featuresâ. In: Proceedings of the seventh IEEE international conference on computer vision. Vol. 2. Ieee. 1999, p. 1150â1157. [88]T. Lozano Perez. âSpatial planning: a configuration space approachâ. In: Autonomous Robot Vehicles. 1990. [89]F. Lu and E. Milios. âRobot pose estimation in unknown environments by matching 2d range scansâ. In: Journal of Intelligent and Robotic systems 18.3 (1997), p. 249â275. [90]K. M. Lynch and K. C. Park. Modern Robotics: Mechanics, Planning, and Control. Cambridge University Press, 2017. Chap. 8. [91]R. Mahler. Statistical multisource-multitarget information fusion. Artech, 2007. [92]V. Makoviychuk et al. âIsaac Gym: High Performance GPU Based Physics Simulation For Robot Learningâ. In: Proceedings of the Neural Information Processing Systems Track on Datasets and Benchmarks. 2021. [93]P. S. Maybeck. Stochastic models, estimation, and control. Vol. 3. Academic press, 1982. [94]V. Mnih et al. âAsynchronous Methods for Deep Reinforcement Learn- ingâ. In: Proceedings of The 33rd International Conference on Machine Learn- ing. 2016, p. 1928â1937. [95]V. Mnih et al. âPlaying Atari with Deep Reinforcement Learningâ. In: ArXiv abs/1312.5602 (2013). url: https://api.semanticscholar.org/ CorpusID:15238391. REFERENCES 525 [96]M. Montemerlo et al. âFastSLAM 2.0: An Improved Particle Filtering Al- gorithm for Simultaneous Localization and Mapping that Provably Con- vergesâ. In: Proceedings of the 18th National Conference on Artificial Intelli- gence (AAAI). 2003, p. 1151â1156. [97]M. Montemerlo et al. âFastSLAM: A factored solution to the simultaneous localization and mapping problemâ. In: Aaai/iaai 593598.2 (2002), p. 593â 598. [98]H. P. Moravec. âTowards automatic visual obstacle avoidanceâ. In: 5th International Joint Conference on Artificial Intelligence. 1977. [99]R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos. âORB-SLAM: A versatile and accurate monocular SLAM systemâ. In: IEEE transactions on robotics 31.5 (2015), p. 1147â1163. [100]K. P. Murphy. Probabilistic Machine Learning: An introduction. MIT Press, 2022. [101]R. M. Murray. Optimization-Based Control. California Institute of Technol- ogy, 2009. [102]A. Ng and S. Russell. âAlgorithms for Inverse Reinforcement Learningâ. In: Proceedings of the Seventeenth International Conference on Machine Learn- ing. 2000, p. 663â670. [103]NVIDIA. âAlpamayo-R1: Bridging Reasoning and Action Prediction for Generalizable Autonomous Driving in the Long Tailâ. In: arXiv preprint arXiv:2511.00088 (2025). [104]NVIDIA. âCosmos World Foundation Model Platform for Physical AIâ. In: arXiv preprint arXiv:2501.03575 (2025). [105]NVIDIA. NVIDIA Halos: Autonomous Vehicle Safety. 2024. url: https : / / w . nvidia . com / en - us / ai - trust - center / halos / autonomous - vehicles/. [106]R. Olfati-Saber, J. A. Fax, and R. M. Murray. âConsensus and Coopera- tion in Networked Multi-Agent Systemsâ. In: Proceedings of the IEEE 95.1 (2007), p. 215â233. [107]T. Osa et al. âAn Algorithmic Perspective on Imitation Learningâ. In: (2018). url: https://arxiv.org/abs/1811.06711. [108]N. Perveen, D. Kumar, and I. Bhardwaj. âAn overview on template match- ing methodologies and its applicationsâ. In: International Journal of Research in Computer and Communication Technology 2.10 (2013), p. 988â995. [109]W. B. Powell. âAI, OR and control theory: A Rosetta Stone for stochastic optimizationâ. In: Princeton University. 2012. [110]M. Puterman. Markov Decision Processes: Discrete Stochastic Dynamic Pro- gramming. Wiley, 2014. 526 principles of robot autonomy [111]C. R. Qi et al. âPointnet: Deep learning on point sets for 3d classification and segmentationâ. In: Proceedings of the IEEE conference on computer vision and pattern recognition. 2017, p. 652â660. [112]C. R. Qi et al. âPointnet++: Deep hierarchical feature learning on point sets in a metric spaceâ. In: Advances in neural information processing systems 30 (2017). [113]T. Qin, P. Li, and S. Shen. âVins-mono: A robust and versatile monocular visual-inertial state estimatorâ. In: IEEE transactions on robotics 34.4 (2018), p. 1004â1020. [114]M. Quigley, B. Gerkey, and W. D. Smart. Programming Robots with ROS: A Practical Introduction to the Robot Operating System. OâReilly Media, 2015. [115]A. Radford et al. âLanguage Models are Unsupervised Multitask Learn- ersâ. In: (2019). [116]A. Radford et al. âLearning Transferable Visual Models From Natural Lan- guage Supervisionâ. In: Proceedings of the 38th International Conference on Machine Learning. 2021, p. 8748â8763. [117]A. Rao. âA Survey of Numerical Methods for Optimal Controlâ. In: Ad- vances in the Astronautical Sciences 135 (2010). [118]N. Ratliff, J. A. Bagnell, and M. Zinkevich. âMaximum Margin Planningâ. In: Proceedings of the 23rd International Conference on Machine Learning. 2006, p. 729â736. [119]H. E. Rauch, F. Tung, and C. T. Striebel. âMaximum likelihood estimates of linear dynamic systemsâ. In: AIAA journal 3.8 (1965), p. 1445â1450. [120]J. Rawlings, D. Q. Mayne, and M. Diehl. Model Predictive Control: Theory, Computation, and Design. Nob Hill Publishing, 2017. [121]J. Redmon et al. âYou Only Look Once: Unified, Real-Time Object Detec- tionâ. In: Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR). June 2016. [122]E. Rimon. Exact robot navigation using artificial potential functions. PhD The- sis. Yale University, 1990. [123]A. Rives et al. âBiological structure and function emerge from scaling unsupervised learning to 250 million protein sequencesâ. In: Proceedings of the National Academy of Sciences 118.15 (2021). [124]O. Ronneberger, P. Fischer, and T. Brox. âU-Net: Convolutional Networks for Biomedical Image Segmentationâ. In: CoRR abs/1505.04597 (2015). arXiv: 1505.04597. url: http://arxiv.org/abs/1505.04597. [125]S. Ross, G. Gordon, and D. Bagnell. âA Reduction of Imitation Learning and Structured Prediction to No-Regret Online Learningâ. In: Proceedings of the Fourteenth International Conference on Artificial Intelligence and Statis- tics. 2011, p. 627â635. REFERENCES 527 [126]S. Sagawa et al. âDistributionally Robust Neural Networks for Group Shifts: On the Importance of Regularization for Worst-Case Generaliza- tionâ. In: Proceedings of the International Conference on Learning Representa- tions. 2020. [127]E. Schmerling and M. Pavone. âKinodynamic Planningâ. In: Encyclopedia of Robotics. First. Springer, 2019. [128]J. A. Sethian. âA fast marching level set method for monotonically advanc- ing frontsâ. In: Proceedings of the National Academy of Sciences 93.4 (1996), p. 1591â1595. [129]S. Shi, X. Wang, and H. Li. âPointRCNN: 3D Object Proposal Generation and Detection From Point Cloudâ. In: The IEEE Conference on Computer Vision and Pattern Recognition (CVPR). June 2019. [130]M. D. Shuster. âSurvey of attitude representationsâ. In: Journal of the As- tronautical Sciences 41.4 (1993), p. 439â517. [131]B. Siciliano and O. Khatib. Springer Handbook of Robotics. Springer-Verlag, 2007. [132]B. Siciliano et al. Robotics: Modelling, Planning and Control. Springer Pub- lishing Company, Incorporated, 2008. Chap. 2. [133]B. Siciliano et al. Robotics: Modelling, Planning and Control. Springer Pub- lishing Company, Incorporated, 2008. Chap. 7. [134]B. Siciliano et al. Robotics: Modelling, Planning and Control. Springer Pub- lishing Company, Incorporated, 2008. [135]R. Siegwart, I. R. Nourbakhsh, and D. Scaramuzza. Introduction to Au- tonomous Mobile Robots. MIT Press, 2011. [136]D. Silver et al. âMastering the game of Go with deep neural networks and tree searchâ. In: Nature 529.7587 (2016), p. 484â489. [137]D. Simon. Optimal State Estimation: Kalman, H â , and Nonlinear Approaches. John Wiley & Sons, 2006. [138]R. Sinha et al. âReal-Time Anomaly Detection and Reactive Planning with Large Language Modelsâ. In: Proceedings of Robotics: Science and Systems. 2024. [139]M. Sipser. Introduction to the Theory of Computation. International Thomson Publishing, 1996. [140]R. Smith, M. Self, and P. Cheeseman. âEstimating uncertain spatial rela- tionships in roboticsâ. In: Autonomous robot vehicles. Springer, 1990, p. 167â 193. [141]A. Stentz. âThe focussed D* algorithm for real-time replanningâ. In: 14th International Joint Conference on Artificial Intelligence. 1995, p. 1652â1659. [142]L. D. Stone et al. Bayesian multiple target tracking. Artech House, 2013. 528 principles of robot autonomy [143]R. Sutton and A. Barto. Reinforcement learning: An introduction. MIT Press, 2018. [144]R. S. Sutton. âDyna, an integrated architecture for learning, planning, and reactingâ. In: SIGART Bull. 2.4 (1991), p. 160â163. [145]R. Szeliski. Computer vision: algorithms and applications. Springer Science & Business Media, 2010. [146]Y. Tassa. Theory and Implementation of Biomimetic Motor Controllers. PhD Thesis. The Hebrew University of Jerusalem, 2011. [147]A. Taylor et al. âLearning for Safety-Critical Control with Control Barrier Functionsâ. In: Learning for Dynamics and Control. 2020, p. 708â717. [148]S. Thrun, W. Burgard, and D. Fox. Probabilistic Robotics. MIT Press, 2005. [149]X. Tian et al. âOcc3D: A Large-Scale 3D Occupancy Prediction Benchmark for Autonomous Drivingâ. In: arXiv preprint arXiv:2304.14365 (2023). [150]A. Torralba, P. Isola, and W. T. Freeman. Foundations of Computer Vision. Available under C-BY-ND-NC license. Cambridge, MA: The MIT Press, 2024. isbn: 978-0-262-04897-2. [151]B. Triggs et al. âBundle adjustmentâa modern synthesisâ. In: Vision Al- gorithms: Theory and Practice: International Workshop on Vision Algorithms Corfu, Greece, September 21â22, 1999 Proceedings. Springer. 2000, p. 298â 372. [152]R. Tsai. âA Versatile Camera Calibration Technique for High-accuracy 3D Machine Vision Metrology Using Off-the-shelf TV Cameras and Lensesâ. In: IEEE Journal on Robotics and Automation 3.4 (1987), p. 323â344. [153]J. R. R. Uijlings et al. âSelective Search for Object Recognitionâ. In: Inter- national Journal of Computer Vision (2013). [154]A. Vaswani et al. âAttention is All you Needâ. In: Advances in Neural In- formation Processing Systems. Vol. 30. Curran Associates, Inc., 2017. [155]B.-N. Vo and W.-K. Ma. âThe Gaussian mixture probability hypothesis density filterâ. In: IEEE Transactions on signal processing 54.11 (2006), p. 4091â 4104. [156]B.-N. Vo, B.-T. Vo, and D. Phung. âLabeled random finite sets and the Bayes multi-target tracking filterâ. In: IEEE Transactions on Signal Process- ing 62.24 (2014), p. 6554â6567. [157]Y. Wang et al. âDynamic graph cnn for learning on point cloudsâ. In: ACM Transactions on Graphics (tog) 38.5 (2019), p. 1â12. [158]C. J. C. H. Watkins and P. Dayan. âQ-learningâ. In: Machine Learning 8.3 (1992), p. 279â292. [159]R. J. Williams. âSimple statistical gradient-following algorithms for con- nectionist reinforcement learningâ. In: Machine Learning 8.3 (1992), p. 229â 256. REFERENCES 529 [160]Y. Yan, Y. Mao, and B. Li. âSecond: Sparsely embedded convolutional detectionâ. In: Sensors 18.10 (2018), p. 3337. [161]M. Zawalski et al. âRobotic Control via Embodied Chain-of-Thought Rea- soningâ. In: Proceedings of The 8th Conference on Robot Learning. 2024. [162]M. D. Zeiler and R. Fergus. âVisualizing and Understanding Convolu- tional Networksâ. In: European Conference on Computer Vision (ECCV). Springer, 2014, p. 818â833. [163]J. Zhang, S. Singh, et al. âLOAM: Lidar odometry and mapping in real- time.â In: Robotics: Science and systems. Vol. 2. 9. Berkeley, CA. 2014, p. 1â 9. [164]S. Zhang, Y. Bar-Shalom, and G. Watson. âTracking with multisensor out- of-sequence measurements with residual biasesâ. In: 2010 13th Interna- tional Conference on Information Fusion. IEEE. 2010, p. 1â8. [165]Z. Zhang. âA Flexible New Technique for Camera Calibrationâ. In: IEEE Transactions on Pattern Analysis and Machine Intelligence 22 (2000). [166]Z. Zhang. âIterative point matching for registration of free-form curves and surfacesâ. In: International journal of computer vision 13.2 (1994), p. 119â 152. [167]Y. Zhou and O. Tuzel. âVoxelnet: End-to-end learning for point cloud based 3d object detectionâ. In: Proceedings of the IEEE conference on computer vision and pattern recognition. 2018, p. 4490â4499. [168]B. D. Ziebart et al. âMaximum Entropy Inverse Reinforcement Learningâ. In: Proceedings of the Twenty-Third AAAI Conference on Artificial Intelligence. 2008, p. 1433â1438. [169]B. Zitkovich et al. âRT-2: Vision-Language-Action Models Transfer Web Knowledge to Robotic Controlâ. In: Proceedings of The 7th Conference on Robot Learning. 2023, p. 2165â2183.