Consensus is a popular technique for distributed state estimation. This formulation allows networks of connected agents or sensors to exchange information about the distribution of a set of targets with their immediate neighbors without the need of a centralized node or layer. We present decentralized consensus-based fusion techniques for a system whose target prior estimates are a weighted mixture of Gaussian probability density functions (PDFs) for the following cases: 1) in which all agents have the same a priori Gaussian mixture estimate of the target, and 2) in which agents have different a priori Gaussian mixture estimates of the target. For the second case, we present a formulation that fuses each agent's a priori estimate without using local observations such that each agent's posterior estimate is the same across the network.
Point-mass filters solve Bayesian recursive relations by approximating probability density functions of a system state over grids of discrete points. The approach suffers from the curse of dimensionality. The exponential increase of the number of the grid points can be mitigated by application of low-rank approximations of multidimensional arrays. Tensor train decompositions represent individual values by the product of matrices. This paper focuses on selected issues that are substantial in state estimation. Namely, the contamination of the density approximations by negative values is discussed first. Functional decompositions of quadratic functions are compared with decompositions of discretised Gaussian densities next. In particular, the connection of correlation with tensor train ranks is explored. Last, the consequences of interpolating the density values from one grid to a new grid are analysed.
Accurate relative orbit determination is a key challenge in modern space operations, particularly when relying on angle-only measurements. The inherent observability limitations of this approach make initial state estimation difficult, impacting mission safety and performance. This work explores the use of active learning (AL) techniques to enhance observability by dynamically designing the input excitation signal offline and at runtime. Our approach leverages AL to design the input signal dynamically, enhancing the observability of the system without requiring additional hardware or predefined maneuvers. We incorporate a dual control technique to ensure target tracking while maintaining observability. The proposed method is validated through numerical simulations, demonstrating its effectiveness in estimating the initial relative state of the chaser and target spacecrafts and its robustness to various initial relative distances and observation periods.
Developing robust, efficient navigation algorithms is challenging. Rule-based methods offer interpretability and modularity but struggle with learning from large datasets, while end-to-end neural networks excel in learning but lack transparency and modularity. In this paper, we present MIND-Stack, a modular software stack consisting of a localization network and a Stanley Controller with intermediate human interpretable state representations and end-to-end differentiability. Our approach enables the upstream localization module to reduce the downstream control error, extending its role beyond state estimation. Unlike existing research on differentiable algorithms that either lack modules of the autonomous stack to span from sensor input to actuator output or real-world implementation, MIND-Stack offers both capabilities. We conduct experiments that demonstrate the ability of the localization module to reduce the downstream control loss through its end-to-end differentiability while offering better performance than state-of-the-art algorithms. We showcase sim-to-real capabilities by deploying the algorithm on a real-world embedded autonomous platform with limited computation power and demonstrate simultaneous training of both the localization and controller towards one goal. While MIND-Stack shows good results, we discuss the incorporation of additional modules from the autonomous navigation pipeline in the future, promising even greater stability and performance in the next iterations of the framework.
This article investigates the problem of data-driven state estimation for linear systems with both unknown system dynamics and noise covariances. We propose an Autocovariance Least-squares-based Data-driven Kalman Filter (ADKF), which provides a unified framework for simultaneous system identification and state estimation by utilizing pre-collected input-output trajectories and estimated initial states. Specifically, we design a SDP-based algorithm for estimating the noise covariances. We quantify the impact of model inaccuracy on noise covariances estimation using this identification algorithm, and introduce a feedback control mechanism for data collection to enhance the accuracy and stability of noise covariance estimation. The estimated noise covariances account for model inaccuracy, which are shown to be more suitable for state estimation. We also quantify the performance gap between the ADKF and the traditional Kalman filter with known system dynamics and noise covariances, showing that this gap decreases as the number and length of pre-collected trajectories increase. Finally, numerical simulations validate the robustness and effectiveness of the proposed ADKF.
Knowledge tracing (KT) aims to predict learners' future performance based on historical learning interactions. However, existing KT models predominantly focus on data from a single course, limiting their ability to capture a comprehensive understanding of learners' knowledge states. In this paper, we propose TransKT, a contrastive cross-course knowledge tracing method that leverages concept graph guided knowledge transfer to model the relationships between learning behaviors across different courses, thereby enhancing knowledge state estimation. Specifically, TransKT constructs a cross-course concept graph by leveraging zero-shot Large Language Model (LLM) prompts to establish implicit links between related concepts across different courses. This graph serves as the foundation for knowledge transfer, enabling the model to integrate and enhance the semantic features of learners' interactions across courses. Furthermore, TransKT includes an LLM-to-LM pipeline for incorporating summarized semantic features, which significantly improves the performance of Graph Convolutional Networks (GCNs) used for knowledge transfer. Additionally, TransKT employs a contrastive objective that aligns single-course and cross-course knowledge states, thereby refining the model's ability to provide a more robust and accurate representation of learners' overall knowledge states.
Spatiotemporal reasoning plays a key role in Cyber-Physical Systems (CPS). Despite advances in Large Language Models (LLMs) and Large Reasoning Models (LRMs), their capacity to reason about complex spatiotemporal signals remains underexplored. This paper proposes a hierarchical SpatioTemporal reAsoning benchmaRK, STARK, to systematically evaluate LLMs across three levels of reasoning complexity: state estimation (e.g., predicting field variables, localizing and tracking events in space and time), spatiotemporal reasoning over states (e.g., inferring spatial-temporal relationships), and world-knowledge-aware reasoning that integrates contextual and domain knowledge (e.g., intent prediction, landmark-aware navigation). We curate 26 distinct spatiotemporal tasks with diverse sensor modalities, comprising 14,552 challenges where models answer directly or by Python Code Interpreter. Evaluating 3 LRMs and 8 LLMs, we find LLMs achieve limited success in tasks requiring geometric reasoning (e.g., multilateration or triangulation), particularly as complexity increases. Surprisingly, LRMs show robust performance across tasks with various levels of difficulty, often competing or surpassing traditional first-principle-based methods. Our results show that in reasoning tasks requiring world knowledge, the performance gap between LLMs and LRMs narrows, with some LLMs even surpassing LRMs. However, the LRM o3 model continues to achieve leading performance across all evaluated tasks, a result attributed primarily to the larger size of the reasoning models. STARK motivates future innovations in model architectures and reasoning paradigms for intelligent CPS by providing a structured framework to identify limitations in the spatiotemporal reasoning of LLMs and LRMs.
State estimation is challenging for 3D object tracking with high maneuverability, as the target's state transition function changes rapidly, irregularly, and is unknown to the estimator. Existing work based on interacting multiple model (IMM) achieves more accurate estimation than single-filter approaches through model combination, aligning appropriate models for different motion modes of the target object over time. However, two limitations of conventional IMM remain unsolved. First, the solution space of the model combination is constrained as the target's diverse kinematic properties in different directions are ignored. Second, the model combination weights calculated by the observation likelihood are not accurate enough due to the measurement uncertainty. In this paper, we propose a novel framework, DIMM, to effectively combine estimates from different motion models in each direction, thus increasing the 3D object tracking accuracy. First, DIMM extends the model combination solution space of conventional IMM from a hyperplane to a hypercube by designing a 3D-decoupled multi-hierarchy filter bank, which describes the target's motion with various-order linear models. Second, DIMM generates more reliable combination weight matrices through a differentiable adaptive fusion network for importance allocation rather than solely relying on the observation likelihood; it contains an attention-based twin delayed deep deterministic policy gradient (TD3) method with a hierarchical reward. Experiments demonstrate that DIMM significantly improves the tracking accuracy of existing state estimation methods by 31.61%~99.23%.
Sparse wearable inertial measurement units (IMUs) have gained popularity for estimating 3D human motion. However, challenges such as pose ambiguity, data drift, and limited adaptability to diverse bodies persist. To address these issues, we propose UMotion, an uncertainty-driven, online fusing-all state estimation framework for 3D human shape and pose estimation, supported by six integrated, body-worn ultra-wideband (UWB) distance sensors with IMUs. UWB sensors measure inter-node distances to infer spatial relationships, aiding in resolving pose ambiguities and body shape variations when combined with anthropometric data. Unfortunately, IMUs are prone to drift, and UWB sensors are affected by body occlusions. Consequently, we develop a tightly coupled Unscented Kalman Filter (UKF) framework that fuses uncertainties from sensor data and estimated human motion based on individual body shape. The UKF iteratively refines IMU and UWB measurements by aligning them with uncertain human motion constraints in real-time, producing optimal estimates for each. Experiments on both synthetic and real-world datasets demonstrate the effectiveness of UMotion in stabilizing sensor data and the improvement over state of the art in pose accuracy.
Block-Oriented Nonlinear (BONL) models, particularly Wiener models, are widely used for their computational efficiency and practicality in modeling nonlinear behaviors in physical systems. Filtering and smoothing methods for Wiener systems, such as particle filters and Kalman-based techniques, often struggle with computational feasibility or accuracy. This work addresses these challenges by introducing a novel Gaussian Sum Filter for Wiener system state estimation that is built on a Gauss-Legendre quadrature approximation of the likelihood function associated with the output signal. In addition to filtering, a two-filter smoothing strategy is proposed, enabling accurate computation of smoothed state distributions at single and consecutive time instants. Numerical examples demonstrate the superiority of the proposed method in balancing accuracy and computational efficiency compared to traditional approaches, highlighting its benefits in control, state estimation and system identification, for Wiener systems.
This paper presents a novel online system identification approach utilizing the Chebyshev pseudospectral (PS) method to approximate the dynamics of a continuous-time nonlinear system. Unlike conventional periodic sampling, the proposed identification scheme employs aperiodic state sampling, leveraging the Chebyshev nodes to achieve the desired approximation accuracy. Unlike traditional off-line PS approaches, the scheme utilizes a moving time-window strategy to compute the sampling instants (Chebyshev nodes) forward in time for state measurements. Within each time window, the number of sampling instants is adaptively determined to meet the specified approximation accuracy. The Chebyshev basis is also shifted for each time window to accommodate arbitrary approximation intervals. The least-squares approach is employed to estimate the coefficients of the shifted Chebyshev basis functions using the measured state and its derivatives at the end of each window, resulting in a piecewise approximation of the drift dynamics of the system. In the second step, the identified drift dynamics are utilized to design an adaptive state estimator to reconstruct the continuous system states from the aperiodic state measurements. To address the smoothness of the piecewise approximated system dynamics, the Chebyshev coefficients are recomputed at the window transition instants, between two consecutive time windows, by enforcing continuity of the approximated function and its derivatives. In addition, analytical results are provided to determine the number of sampling instants within each time window, which guarantees the desired approximation accuracy. The boundedness of function approximation and state estimation errors is also proved analytically. Finally, numerical simulation results are included to validate the proposed scheme.
Filtering is crucial in engineering fields, providing vital state estimation for control systems. However, the nonlinear nature of complex systems and the presence of non-Gaussian noises pose significant challenges to the performance of conventional filtering methods in terms of estimation accuracy and computational efficiency. In this work, we present a data-driven closed-loop filter, termed datatic approximate optimal filter (DAOF), specifically designed for nonlinear systems under non-Gaussian conditions. We first formulate a Markovian filtering problem (MFP), which inherently shares a connection with reinforcement learning (RL) as it aims to compute the optimal state estimate by minimizing the accumulated error. To solve MFP, we propose DAOF, which primarily incorporates a trained RL policy and features two distinct structural designs: DAOF-v1 and DAOF-v2. Designed for systems with explicit models, DAOF-v1 combines prediction and update phases, with the RL policy generating the update value. Meanwhile, DAOF-v2 bypasses system modeling by directly outputting the state estimate. Then, we utilize an actor-critic algorithm to learn the parameterized policy for DAOF. Experimental results on a 2-degree-of-freedom (2-DOF) vehicle system, equipped with explicit system models, demonstrate the superior accuracy and computational efficiency of DAOF-v1 compared to existing nonlinear filters. Moreover, DAOF-v2 showcases its unique ability to perform filtering without requiring explicit system modeling, as validated by a 14-DOF vehicle system.
This paper presents a secure safety filter design for nonlinear systems under sensor spoofing attacks. Existing approaches primarily focus on linear systems which limits their applications in real-world scenarios. In this work, we extend these results to nonlinear systems in a principled way. We introduce exact observability maps that abstract specific state estimation algorithms and extend them to a secure version capable of handling sensor attacks. Our generalization also applies to the relaxed observability case, with slightly relaxed guarantees. More importantly, we propose a secure safety filter design in both exact and relaxed cases, which incorporates secure state estimation and a control barrier function-enabled safety filter. The proposed approach provides theoretical safety guarantees for nonlinear systems in the presence of sensor attacks. We numerically validate our analysis on a unicycle vehicle equipped with redundant yet partly compromised sensors.
This paper presents a visual-inertial odometry (VIO) method using long-tracked features. Long-tracked features can constrain more visual frames, reducing localization drift. However, they may also lead to accumulated matching errors and drift in feature tracking. Current VIO methods adjust observation weights based on re-projection errors, yet this approach has flaws. Re-projection errors depend on estimated camera poses and map points, so increased errors might come from estimation inaccuracies, not actual feature tracking errors. This can mislead the optimization process and make long-tracked features ineffective for suppressing localization drift. Furthermore, long-tracked features constrain a larger number of frames, which poses a significant challenge to real-time performance of the system. To tackle these issues, we propose an active decoupling mechanism for accumulated errors in long-tracked feature utilization. We introduce a visual reference frame reset strategy to eliminate accumulated tracking errors and a depth prediction strategy to leverage the long-term constraint. To ensure real time preformane, we implement three strategies for efficient system state estimation: a parallel elimination strategy based on predefined elimination order, an inverse-depth elimination simplification strategy, and an elimination skipping strategy. Experiments on various datasets show that our method offers higher positioning accuracy with relatively short consumption time, making it more suitable for edge-enabled low-altitude IoT navigation, where high-accuracy positioning and real-time operation on edge device are required. The code will be published at github.
Manipulating clothes is challenging due to their complex dynamics, high deformability, and frequent self-occlusions. Garments exhibit a nearly infinite number of configurations, making explicit state representations difficult to define. In this paper, we analyze BiFold, a model that predicts language-conditioned pick-and-place actions from visual observations, while implicitly encoding garment state through end-to-end learning. To address scenarios such as crumpled garments or recovery from failed manipulations, BiFold leverages temporal context to improve state estimation. We examine the internal representations of the model and present evidence that its fine-tuning and temporal context enable effective alignment between text and image regions, as well as temporal consistency.
This work addresses the critical lack of precision in state estimation in the Kalman filter for 3D multi-object tracking (MOT) and the ongoing challenge of selecting the appropriate motion model. Existing literature commonly relies on constant motion models for estimating the states of objects, neglecting the complex motion dynamics unique to each object. Consequently, trajectory division and imprecise object localization arise, especially under occlusion conditions. The core of these challenges lies in the limitations of the current Kalman filter formulation, which fails to account for the variability of motion dynamics as objects navigate their environments. This work introduces a novel formulation of the Kalman filter that incorporates motion dynamics, allowing the motion model to adaptively adjust according to changes in the object's movement. The proposed Kalman filter substantially improves state estimation, localization, and trajectory prediction compared to the traditional Kalman filter. This is reflected in tracking performance that surpasses recent benchmarks on the KITTI and Waymo Open Datasets, with margins of 0.56\% and 0.81\% in higher order tracking accuracy (HOTA) and multi-object tracking accuracy (MOTA), respectively. Furthermore, the proposed Kalman filter consistently outperforms the baseline across various detectors. Additionally, it shows an enhanced capability in managing long occlusions compared to the baseline Kalman filter, achieving margins of 1.22\% in higher order tracking accuracy (HOTA) and 1.55\% in multi-object tracking accuracy (MOTA) on the KITTI dataset. The formulation's efficiency is evident, with an additional processing time of only approximately 0.078 ms per frame, ensuring its applicability in real-time applications.
As robots become more generalized and deployed in diverse environments, they must interact with complex objects, many with multiple independent joints or degrees of freedom (DoF) requiring precise control. A common strategy is object modeling, where compact state-space models are learned from real-world observations and paired with classical planning. However, existing methods often rely on prior knowledge or focus on single-DoF objects, limiting their applicability. They also fail to handle occluded joints and ignore the manipulation sequences needed to access them. We address this by learning object models from human demonstrations. We introduce Object Kinematic Sequence Machines (OKSMs), a novel representation capturing both kinematic constraints and manipulation order for multi-DoF objects. To estimate these models from point cloud data, we present Pokenet, a deep neural network trained on human demonstrations. We validate our approach on 8,000 simulated and 1,600 real-world annotated samples. Pokenet improves joint axis and state estimation by over 20 percent on real-world data compared to prior methods. Finally, we demonstrate OKSMs on a Sawyer robot using inverse kinematics-based planning to manipulate multi-DoF objects.
Machine learning has become a powerful tool for enhancing data assimilation. While supervised learning remains the standard method, reinforcement learning (RL) offers unique advantages through its sequential decision-making framework, which naturally fits the iterative nature of data assimilation by dynamically balancing model forecasts with observations. We develop RL-DAUNCE, a new RL-based method that enhances data assimilation with physical constraints through three key aspects. First, RL-DAUNCE inherits the computational efficiency of machine learning while it uniquely structures its agents to mirror ensemble members in conventional data assimilation methods. Second, RL-DAUNCE emphasizes uncertainty quantification by advancing multiple ensemble members, moving beyond simple mean-state optimization. Third, RL-DAUNCE's ensemble-as-agents design facilitates the enforcement of physical constraints during the assimilation process, which is crucial to improving the state estimation and subsequent forecasting. A primal-dual optimization strategy is developed to enforce constraints, which dynamically penalizes the reward function to ensure constraint satisfaction throughout the learning process. Also, state variable bounds are respected by constraining the RL action space. Together, these features ensure physical consistency without sacrificing efficiency. RL-DAUNCE is applied to the Madden-Julian Oscillation, an intermittent atmospheric phenomenon characterized by strongly non-Gaussian features and multiple physical constraints. RL-DAUNCE outperforms the standard ensemble Kalman filter (EnKF), which fails catastrophically due to the violation of physical constraints. Notably, RL-DAUNCE matches the performance of constrained EnKF, particularly in recovering intermittent signals, capturing extreme events, and quantifying uncertainties, while requiring substantially less computational effort.
"Read the room" is a significant social reasoning capability in human daily life. Humans can infer others' mental states from subtle social cues. Previous social reasoning tasks and datasets lack complexity (e.g., simple scenes, basic interactions, incomplete mental state variables, single-step reasoning, etc.) and fall far short of the challenges present in real-life social interactions. In this paper, we contribute a valuable, high-quality, and comprehensive video dataset named R^3-VQA with precise and fine-grained annotations of social events and mental states (i.e., belief, intent, desire, and emotion) as well as corresponding social causal chains in complex social scenarios. Moreover, we include human-annotated and model-generated QAs. Our task R^3-VQA includes three aspects: Social Event Understanding, Mental State Estimation, and Social Causal Reasoning. As a benchmark, we comprehensively evaluate the social reasoning capabilities and consistencies of current state-of-the-art large vision-language models (LVLMs). Comprehensive experiments show that (i) LVLMs are still far from human-level consistent social reasoning in complex social scenarios; (ii) Theory of Mind (ToM) prompting can help LVLMs perform better on social reasoning tasks. We provide some of our dataset and codes in supplementary material and will release our full dataset and codes upon acceptance.
Guided wave-based techniques have been used extensively in Structural Health Monitoring (SHM). Models using guided waves can provide information from both time and frequency domains to make themselves accurate and robust. Probabilistic SHM models, which have the ability to account for uncertainties, are developed when decision confidence intervals are of interest. Most active-sensing guided-wave methods rely on the assumption that a large dataset can be collected, making them impractical when data collection is constrained by time or environmental factors. Meanwhile, although simulation results may lack the accuracy of real-world data, they are easier to obtain. In this context, models that integrate data from multiple sources have the potential to combine the accuracy of experimental data with the convenience of simulated data, without requiring large and potentially costly experimental datasets. The goal of this work is to introduce and assess a probabilistic multi-fidelity Gaussian process regression framework for damage state estimation via the use of both experimental and simulated guided waves. The main differences from previous works include the integration of damage-sensitive features (damage indices, DIs) extracted from both experimental and numerical sources, as well as the use of a relatively small amount of real-world data. The proposed model was validated by two test cases where multiple data sources exist. For each test case, experimental data were collected from a piezoelectric sensor network attached to an aluminum plate with various structural conditions, while simulated data were generated using either multiphysics finite element model (FEM) or physics-based signal reconstruction approaches under the same conditions.