💻 computer science

DynoJEPP: Joint Estimation, Prediction and Planning in Dynamic Environments

DynoJEPP is a novel factor-graph framework that ensures safe navigation in dynamic environments by introducing directed factors to prevent prediction and planning from corrupting state estimation, while its cooperative extension further enhances trajectory planning by incorporating the behaviors of other agents.

Original authors: Mikolaj Kliniewski, Jesse Morris, Yiduo Wang, Ian R. Manchester, Viorela Ila

Published 2026-05-14

📖 4 min read☕ Coffee break read

CC BY 4.0

Original authors: Mikolaj Kliniewski, Jesse Morris, Yiduo Wang, Ian R. Manchester, Viorela Ila

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine a robot trying to navigate a busy warehouse. It has three main jobs to do at the same time:

Estimation: "Where am I right now, and where are those moving boxes?"
Prediction: "Where are those boxes going to be in a few seconds?"
Planning: "How should I move to avoid them and get to my destination?"

The paper introduces a new system called DynoJEPP that handles all three jobs together. However, the authors discovered a major problem with how these jobs usually talk to each other, and they invented a clever fix.

The Problem: The "Too Helpful" Friend

In older systems, the robot's "planning" brain and "prediction" brain would chat freely with the "estimation" brain. The problem is that the planning brain is very optimistic. It wants to find a path and will sometimes convince the estimation brain that the robot is in a safer spot than it actually is, or that a moving obstacle is going to stop sooner than it really will.

The authors compare this to a bad feedback loop. If you ask a friend, "Do I look good?" and they say "Yes, you look perfect," you might start believing it even if you have spinach in your teeth. In the robot's case, this "spinach in the teeth" is a dangerous miscalculation. The robot thinks it's safe, but it's actually about to crash.

In their experiments, when the robot let the planning and prediction parts influence the estimation part, it crashed in almost every test.

The Solution: The One-Way Street

To fix this, the authors built a Directed Factor. Think of this as a one-way street or a strict manager in the robot's brain.

The Old Way: Information flowed back and forth. The plan could change the estimate, and the estimate could change the plan.
The New Way (DynoJEPP): Information flows only one way: Estimation → Prediction → Planning.

The "Estimation" part (the truth-teller) looks at the sensors and says, "Here is exactly where we are and where the boxes are." It sends this truth to the "Prediction" part. The Prediction part says, "Okay, based on that truth, the boxes will move here." It sends that to the "Planning" part. The Planning part says, "Got it, I will steer this way to avoid them."

Crucially, the Planning part cannot talk back to the Estimation part. It cannot say, "I think we are actually over here, so the boxes must be over there." The Estimation part remains pure and uncorrupted by the robot's desires to move forward.

The "Cooperative" Upgrade

The paper also introduces a special version called Cooperative DynoJEPP. Imagine the moving obstacles aren't just mindless boxes, but other robots that can also see the ego robot.

In this version, the robot says, "I see you moving, and I know you can see me. If I slow down, you might speed up to pass me." The system allows the robot's plan to gently nudge the prediction of the other object's path. It's like a dance where two partners adjust their steps based on each other, rather than one partner blindly guessing the other's moves. This allows the robot to be bolder and faster, rather than just waiting nervously.

The Results

The authors tested this in a simulated warehouse:

Without the one-way street (Old way): The robot crashed almost every time because it got confused by its own optimistic plans.
With the one-way street (DynoJEPP): The robot successfully navigated through static and moving obstacles without crashing.
With the cooperative upgrade: The robot moved even more efficiently by "negotiating" with other moving objects.

The Bottom Line

The paper claims that for a robot to be safe in a dynamic world, it must keep its "reality check" (estimation) separate from its "dreams" (planning). By using a Directed Factor to block the planning part from messing with the reality check, the robot stays grounded in reality while still being able to plan a safe path. This simple change in how information flows turned a crashing robot into a successful one.

Technical Summary: DynoJEPP – Joint Estimation, Prediction, and Planning in Dynamic Environments

1. Problem Statement

Robotic autonomy in real-world environments is fundamentally challenged by the presence of dynamic objects with complex, unpredictable motions. While recent advances have successfully unified robot state estimation and path planning within factor-graph frameworks for static environments, extending these approaches to dynamic scenes remains limited.

Existing solutions typically face one of two limitations:

Assumption of Perfect Knowledge: They assume future object trajectories are known a priori, which is unrealistic in dynamic settings.
Reactive Strategies: They rely on purely reactive avoidance, lacking the foresight of trajectory prediction.

A critical, often overlooked issue in simultaneous optimization frameworks is the bidirectional flow of information. In conventional joint formulations, information from the planning and prediction modules feeds back into the state estimation module. This feedback loop corrupts state estimates with "optimistic" biases derived from future plans, leading to degraded estimation accuracy, undesired behaviors, and ultimately, unsafe plans and collisions.

2. Methodology: The DynoJEPP Framework

The authors propose DynoJEPP, a unified factor-graph-based framework that simultaneously optimizes robot and object state estimation, trajectory prediction, and local planning. The core innovation lies in the introduction of a novel directed factor to control information flow.

A. Directed Factors

To prevent planning and prediction from degrading estimation, DynoJEPP introduces a directed factor that enforces unidirectional information flow:

Estimation $\to$ Prediction/Planning: Estimation results (current robot pose and object motions) propagate forward to inform prediction and planning.
Prediction/Planning $\nrightarrow$ Estimation: Future states and planning constraints are prevented from influencing the estimation of current or past states.

Technically, this is achieved via Jacobian manipulation. By zeroing the Jacobian entries with respect to the source variable (the estimation state) in the normal equations ( $J^\top J \delta = -J^\top r$ ), the framework ensures that the gradient of the planning/prediction cost does not update the estimation variables. This structurally decouples the estimation sub-graph from the planning sub-graph while maintaining the necessary dependency for planning to utilize current estimates.

B. Joint Formulation Components

The framework integrates three modules into a single factor graph:

State Estimation: Builds upon Hybrid Dynamic SLAM to estimate the robot pose ( $W X_k$ ) and the SE(3) rigid-body motions of dynamic objects ( $W eH_k$ ). It utilizes point measurements, odometry, and motion smoothing factors to track object centers of mass.
Object Motion Prediction: Utilizes the estimated SE(3) motions to forecast future object trajectories over a horizon $N$ . It employs a constant velocity model (enforced by smoothing factors) but relaxes this assumption near static obstacles using Static Scene-Aware factors (ESDF-based).
Local Planning: Employs a factor-graph-based Model Predictive Control (MPC) to optimize future robot poses, velocities, and accelerations. It incorporates:
- Static Scene Awareness: Uses Euclidean Signed Distance Fields (ESDF) to avoid static obstacles.
- Dynamic Scene Awareness: Uses directed dynamic obstacle factors to penalize proximity to moving objects based on their predicted trajectories.

C. Cooperative Extension (C-DynoJEPP)

The paper introduces Cooperative DynoJEPP (C-DynoJEPP), which allows the ego robot to incorporate cooperative behaviors into its planning. In this extension, the dynamic obstacle factors are directed from planning to prediction. This creates a directed cycle between prediction and planning, modeling the mutual influence where the robot reacts to the object, and the object is expected to react to the robot's intended path. This enables less conservative, more assertive navigation in interactive scenarios.

3. Key Contributions

DynoJEPP Framework: The first factor-graph-based framework to jointly estimate dynamic object motion, predict their evolution, and incorporate these predictions into robot control actions within a single optimization problem.
Novel Directed Factor: A mechanism to specify influence direction in factor graphs, allowing controlled information flow between estimation, prediction, and planning. This prevents the "corruption" of state estimates by future planning constraints.
Cooperative Optimization: An extension (C-DynoJEPP) that parameterizes the level of dynamic object cooperation, enabling mutual influence between the robot's plan and the object's prediction without compromising the independence of state estimation.

4. Experimental Results

The authors evaluated the framework in a simulated warehouse environment (Gazebo) with differential drive robots and dynamic obstacles. They compared three configurations: Undirected (bidirectional flow), Directed (DynoJEPP), and Decoupled (separate estimation followed by joint prediction/planning).

Safety and Success Rate: In 10 navigation experiments involving dynamic obstacles, the Undirected configuration failed in the majority of cases (colliding in 8 out of 10 dynamic trials) due to overly optimistic planning corrupting estimates. In contrast, the Directed configuration achieved a 100% success rate across all trials.
Estimation Accuracy: The Directed method maintained estimation accuracy comparable to the Decoupled baseline, significantly outperforming the Undirected approach. The Undirected method showed significant degradation in both robot and object trajectory estimates.
Cooperative Navigation: In scenarios with reactive dynamic obstacles, C-DynoJEPP demonstrated the ability to navigate more assertively than standard DynoJEPP. It reached goals faster (82 steps vs. 92 steps for the fastest DynoJEPP run) by leveraging cooperative weights to influence predictions.

5. Significance and Claims

The paper claims that controlling the directionality of information flow is critical for safe operation in dynamic environments. Without directed factors, the coupling of planning and estimation leads to a feedback loop where planning objectives distort state estimates, resulting in unsafe behaviors.

The authors emphasize that while estimation does not inherently benefit from joint optimization with planning, prediction and planning do benefit from being jointly optimized with estimation, particularly when modeling interactive behaviors. The proposed framework provides a robust solution that enables safe, simultaneous optimization without sacrificing the integrity of state estimation.

The work concludes by noting that future research will focus on implementing an efficient, incremental, and multi-threaded version of DynoJEPP to achieve real-time performance, benchmarking against industry standards like Nav2.