Orientation estimation is crucial for the successful operation of robots in autonomous control, enabling effective navigation, environmental interaction, and precise task execution. Obtaining a low-error orientation estimate promptly is essential for ensuring safe and successful autonomous robot operation. However, existing approaches, such as the Error-State Kalman Filter (ESKF), suffer from slow convergence, leading to unstable flights and increased collision risks, particularly when initial heading alignment errors occur. We introduce an Invariant Extended Kalman Filter (InEKF) for inertial measurement unit (IMU) and ultra-wideband (UWB) sensor fusion that addresses the slow convergence issue in orientation estimation of the Crazyflie Bolt drone. We demonstrate significant improvements over the reference ESKF, achieving a RMS heading error of 0.08 radians (4.6 degrees) compared to the ESKF’s 1.34 radians (77 degrees), a 94% reduction in error, when starting from a 180 degree initial heading error, while maintaining similar accuracy levels in position and velocity estimates.

Connected Autonomous Vehicles (CAVs) are expected to enable reliable, efficient, and intelligent transportation systems. Most motion-planning algorithms for multi-agent systems implicitly assume that all vehicles/agents will execute the expected plan with a small error and evaluate their safety constraints based on this fact. This assumption, however, is hard to keep for CAVs since they may have to change their plan (e.g., to yield to another vehicle) or are forced to stop (e.g., a CAV may break down). While it is desired that a CAV never gets involved in an accident, it may be hit by other vehicles and, sometimes, preventing the accident is impossible (e.g., getting hit from behind while waiting at a red light). Responsibility-Sensitive Safety (RSS) is a set of safety rules that defines the objective of CAVs to blame, instead of safety. Thus, instead of developing a CAV algorithm that will avoid any accident, it ensures that the ego vehicle will not be blamed for any accident it is a part of. Original RSS rules, however, are hard to evaluate for merge, intersection, and unstructured road scenarios, plus RSS rules do not prevent deadlock situations among vehicles. In this article, we propose a new formulation for RSS rules that can be applied to any driving scenario. We integrate the proposed RSS rules with the CAV’s motion planning algorithm to enable cooperative driving of CAVs. We use Control Barrier Functions to enforce safety constraints and compute the energy optimal trajectory for the ego CAV. Finally, to ensure liveness, our approach detects and resolves deadlocks in a decentralized manner. We have conducted different experiments to verify that the ego CAV does not cause an accident no matter when other CAVs slow down or stop. We also showcase our deadlock detection and resolution mechanism using our simulator. Finally, we compare the average velocity and fuel consumption of vehicles when they drive autonomously with the case that they are autonomous and connected.

Many Cyber-Physical Systems (CPS) have timing constraints that must be met by the cyber components (software and the network) to ensure safety. It is a tedious job to check if a CPS meets its timing requirement especially when it is distributed and the software and/or the underlying computing platforms are complex. Furthermore, the system design is brittle since a timing failure can still happen (e.g., network failure, soft error bit flip). In this article, we propose a new design methodology calledPlan Bwhere timing constraints of the CPS are monitored at runtime, and a proper backup routine is executed when a timing failure happens to ensure safety. We provide a model on how to express the desired timing behavior using a set of timing constructs in a C/C++ code and how to efficiently monitor them at the runtime. We showcase the effectiveness of our approach by conducting experiments on three case studies: (1) the full software stack for autonomous driving (Apollo), (2) a multi-agent system with 1/10th-scale model robots, and (3) a quadrotor for search and rescue application. We show that the system remains safe and stable even when intentional faults are injected to cause a timing failure. We also demonstrate that the system can achieve graceful degradation when a less extreme timing failure happens.

As vehicles become autonomous and connected, intelligent management techniques can be utilized to operate an intersection without a traffic light. When a Connected Autonomous Vehicle (CAV) approaches an intersection, it shares its status and intended direction with the Intersection Manager (IM), and the IM checks the status of other CAVs and assigns a target velocity/reference trajectory for it to maintain. In practice, however, there is an unknown delay between the time a CAV sends a request to the IM and the moment it receives back the response, namely, the Round-Trip Delay (RTD). As a result, the CAV will start tracking the target velocity/reference trajectory later than when the IM expects, which may lead to accidents. In this article, we present a time-aware approach, Crossroads+, that makes CAVs’ behaviors deterministic despite the existence of the unknown RTD. In Crossroads+, we use timestamping and synchronization to ensure that both the IM and the CAVs have the same notion of time. The IM will also set a fixed start time to track the target velocity/reference trajectory for each CAV. The effectiveness of the proposed Crossroads+ technique is illustrated by experiments on a 1/10 scale model of an intersection with CAVs. We also built a simulator to demonstrate the scalability of Crossroads+ for multi-lane intersections. Results from our experiments indicate that our approach can reduce the position uncertainty by 15% in comparison with conventional techniques and achieve up to 36% better throughputs.

Intersection management of Connected Autonomous Vehicles (CAVs) has the potential to improve safety and mobility. CAVs approaching an intersection can exchange information with the infrastructure or each other to schedule their cross times. By avoiding unnecessary stops, scheduling CAVs can increase traffic throughput, reduce energy consumption, and most importantly, minimize the number of accidents that happen in intersection areas due to human errors. We study existing intersection management approaches from following key perspectives: (1) intersection management interface, (2) scheduling policy, (3) existing wireless technologies, (4) existing vehicle models used by researchers and their impact, (5) conflict detection, (6) extension to multi-intersection management, (7) challenges of supporting human-driven vehicles, (8) safety and robustness required for real-life deployment, (9) graceful degradation and recovery for emergency scenarios, (10) security concerns and attack models, and (11) evaluation methods. We then discuss the effectiveness and limitations of each approach with respect to the aforementioned aspects and conclude with a discussion on tradeoffs and further research directions.

Formal specifications on temporal behavior of Cyber-Physical Systems (CPS) is essential for verification of performance and safety. Existing solutions for verifying the satisfaction of temporal constraints on a CPS are compute and resource intensive since they require buffering signals from the CPS prior to constraint checking. We present an online approach, based on Timestamp Temporal Logic (TTL), for monitoring the timing constraints in CPS. The approach reduces the computation and memory requirements by processing the timestamps of pertinent events reducing the need to capture the full data set from the signal sampling. The signal buffer size bears a geometric relationship to the dimension of the signal vector, the time interval being considered, and the sampling resolution. Since monitoring logic is typically implemented on Field Programmable Gate Arrays (FPGAs) for efficient monitoring of multiple signals simultaneously, the space required to store the buffered data becomes the limiting resource. The monitoring logic, for the timing constraints on the Flying Paster (a printing application requiring synchronization between two motors), is illustrated in this paper to demonstrate a geometric reduction in memory and computational resources in the realization of an online monitor.

Utilizing intelligent transportation infrastructures can significantly improve the throughput of intersections of Connected Autonomous Vehicles (CAV), where an Intersection Manager (IM) assigns a target velocity to incoming CAVs in order to achieve a high throughput. Since the IM calculates the assigned velocity for a CAV based on the model of the CAV, it's vulnerable to model mismatches and possible external disturbances. As a result, IM must consider a large safety buffer around all CAVs to ensure a safe scheduling, which greatly degrades the throughput. In addition, IM has to assign a relatively lower speed to CAVs that intend to make a turn at the intersection to avoid rollover. This issue reduces the throughput of the intersection even more. In this paper, we propose a space and time-aware technique to manage intersections of CAVs that is robust against external disturbances and model mismatches. In our method, RIM, IM is responsible for assigning a safe Time of Arrival (TOA) and Velocity of Arrival (VOA) to an approaching CAV such that trajectories of CAVs before and inside the intersection does not conflict. Accordingly, CAVs are responsible for determining and tracking an optimal trajectory to reach the intersection at the assigned TOA while driving at VOA. Since CAVs track a position trajectory, the effect of bounded model mismatch and external disturbances can be compensated. In addition, CAVs that intend to make a turn at the intersection do not need to drive at a slow velocity before entering the intersection. Results from conducting experiments on a 1/10 scale intersection of CAVs show that RIM can reduce the position error at the expected TOA by 18X on average in presence of up to 10% model mismatch and an external disturbance with an amplitude of 5% of max range. In total, our technique can achieve 2.7X better throughput on average compared to velocity assignment techniques.

Time is a foundational aspect of Cyber-Physical Systems (CPS). Correct time and timing of system events are critical to optimized responsiveness to the environment, in terms of timeliness, accuracy, and precision in the knowledge, measurement, prediction, and control of CPS behavior. However, both the specification and verification of timing requirements of the CPS are typically done in an ad-hoc manner. While feasible, the system can become costly and difficult to analyze and maintain, and the process of implementing and verifying correct timing behavior can be error-prone. Towards the development of a verification testbed for testing timing behavior in tools and platforms with explicit time support, this paper first describes a way to express the various kinds of timing constraints in distributed CPS. Then, we outline the design and initial implementation of a distributed testbed to verify the timing of a distributed CPS analytically through a systematic framework. Finally, we illustrate the use of the verified timing testbed on two distributed CPS case studies.

In order to test the performance and verify the correctness of Cyber-Physical Systems (CPS), the timing constraints on the system behavior must be met. Signal Temporal Logic (STL) can efficiently and succinctly capture the timing constraints of a given system model. However, many timing constraints on CPS are more naturally expressed in terms of events on signals. While it is possible to specify event-based timing constraints in STL, such statements can quickly become long and arcane in even simple systems. Timing constraints for CPS, which can be large and complex systems, are often associated with tolerances, the expression of which can make the timing constraints even more cumbersome using STL. This paper proposes a new logic, Timestamp Temporal Logic (TTL), to provide a definitional extension of STL that more intuitively expresses the timing constraints of distributed CPS. TTL also allows for a more natural expression of timing tolerances. Additionally, this paper outlines a methodology to automatically generate logic code and programs to monitor the expressed timing constraints. Since our TTL monitoring logic evaluates the timing constraints using only the timestamps of the required events on the signal, the TTL monitoring logic has significantly less memory footprint when compared to traditional STL monitoring logic, which stores the signal value at the required sampling frequency. The key contribution of this paper is a scalable approach for online monitoring of the timing constraints. We demonstrate the capabilities of TTL and our methodology for online monitoring of TTL constraints on two case studies: 1) Synchronization and phase control of two generators and, 2) Simultaneous image capture using distributed cameras for 3D image reconstruction.