Extended Kalman Filter-Based Visual Odometry in Dynamic Environments Using Modified 1-Point RANSAC

Abstract

Visual odometry in dynamic environments is particularly challenging, as moving objects often cause incorrect data associations and large pose estimation errors. Traditional EKF-based VO methods rely on 1-point RANSAC to reject outliers under the assumption of a static world, thereby discarding dynamic landmarks as noise. However, in practice, outliers may arise not only from measurement errors but also from the motion of objects. To address this issue, we propose a modified 1-point RANSAC framework that detects dynamic objects and leverages both static and dynamic landmarks for ego-motion estimation. Inspired by adaptive strategies observed in biological vision systems, our approach integrates EKF-based state estimation with dynamic object tracking to achieve simultaneous ego-motion and object-motion estimation, improving robustness in complex and dynamic scenes.

1. Introduction

Visual odometry (VO) is a method that estimates the ego-pose, i.e., the position and orientation of an agent, using image data, and it has been widely applied in autonomous driving, robotics, and drone navigation. The fundamental concept underlying VO is to infer the agent’s relative motion by analyzing the changes in visual observations across consecutive frames. Similar mechanisms are observed in biological vision, where animals and insects estimate self-motion from optic flow and the relative motion of surrounding objects. Gay et al. [1] presented a navigation model inspired by place and grid cells, and Wang et al. [2] proposed REVIO using event-based vision inspired by retinal ganglion cells. Based on this concept, ego-pose estimation methods can be categorized into three types depending on the nature of data association: 2D-2D, 2D-3D (or equivalently, 3D-2D), and 3D-3D matching [3]. In 2D-2D matching, the relative pose between consecutive frames is estimated by analyzing geometric constraints between 2D feature correspondences extracted from consecutive images and is commonly used in monocular visual odometry [4], but it suffers from scale ambiguity, as depth information is not directly available. In 2D-3D matching, 2D image features are associated with known 3D landmarks obtained via triangulation (e.g., from stereo cameras) or direct depth sensors (e.g., RGB-D). The camera pose can then be estimated by solving the Perspective-n-Point (PnP) problem [5,6]. Finally, 3D-3D matching estimates the sensor’s motion by aligning two consecutive 3D point clouds. Since both point sets are defined in a common metric space, this approach inherently provides metric scale and high-precision motion estimation. It is widely employed in LiDAR-based and stereo camera-based ego-pose estimation pipelines, particularly where depth sensing is reliable and dense [7]. Similar mechanisms are observed in biological vision, where animals and insects estimate self-motion from optic flow and the relative motion of surrounding objects.

Beyond data association types, ego-pose estimation methods can also be classified by the underlying optimization framework used for state estimation. Among these, a common approach is to directly solve a bundle adjustment (BA) [8] problem over a sliding window of keyframes, refining both camera poses and landmarks by minimizing reprojection error. Alternatively, filtering-based methods such as the Extended Kalman Filter (EKF), and graph-based optimization techniques that model the problem as a factor graph, are also widely adopted.

EKF-based methods estimate the current state sequentially by propagating and updating the state vector and covariance using probabilistic models [9]. They are suitable for real-time applications due to their computational efficiency but may suffer from linearization errors and filter inconsistency in highly nonlinear environments. In contrast, graph-based optimization methods formulate the problem as a global nonlinear least-squares optimization over a factor graph, allowing for more accurate and globally consistent solutions by jointly optimizing all poses and measurements [10]. However, they are generally more computationally demanding than filtering methods, and may require additional engineering to meet strict real-time constraints. In recent years, graph-based methods have become dominant in VO and SLAM (Simultaneous Localization and Mapping) because they enable loop closure and globally consistent optimization [11,12,13]. Although EKF-based ego-pose estimation has been less actively studied in recent years, it still offers advantages such as low computational load, real-time recursive updates, and suitability for sensor fusion [14,15].

The presence of measurement noise and outliers significantly affects estimation accuracy. Several approaches have been proposed to distinguish outliers. In graph-based methods, robust graph-based optimization has been widely studied to mitigate the effects of outliers and measurement noise in visual and pose graph SLAM. Huber loss and other M-estimators have been commonly used to suppress the influence of large residuals in the cost function, providing robustness against occasional false matches in data association [16]. Dynamic Covariance Scaling (DCS) further improves robustness by down-weighting erroneous constraints based on their residual magnitudes in a closed-form, enabling efficient real-time operation without introducing additional variables [17]. Switchable Constraints (SC), on the other hand, explicitly model the reliability of each loop closure by introducing a latent switch variable that can deactivate incorrect constraints during optimization, achieving high resilience in the presence of false positives [18]. In EKF-based methods, Joint Compatibility Branch and Bound (JCBB) [19] is used to reject outliers in ego-pose estimation by evaluating all possible measurement associations and selecting the largest subset that meets joint statistical consistency criteria. Compared to JCBB, which performs a full branch-and-bound search over all possible data association hypotheses and selects the largest jointly compatible inlier set, 1-point RANSAC [20] significantly reduces computational cost by generating motion hypotheses from a single randomly selected feature (or landmark), estimating ego-pose through EKF, and identifying inlier features by thresholding their innovation residuals.

Since general algorithms are assumed to be used in static environments, their performance degrades when dynamic objects are present, leading to incorrect data associations and large pose estimation errors. To address this issue, robust outlier rejection should be applied for ego-pose estimation in dynamic environments. Cao et al. [21] propose a method that integrates optical flow-based motion state estimation into the sliding window optimization of VINS-Mono, enabling robust visual-inertial odometry in dynamic environments without relying on semantic information. Likewise, Li et al. [22] enhance sliding-window BA VIO by incorporating IMU-PARSAC-based matching and robust pure-rotation handling. Ballester et al. [23] integrate segmentation and photometric optimization into ORB-SLAM2 to remove dynamic features and perform BA-based optimization. Similarly, Wu et al. [24] extend ORB-SLAM3 with optical flow segmentation to eliminate dynamic outliers before BA optimization. Ram [25] adds planar homography constraints to the BA framework of VINS-Mono to achieve stable optimization under dynamic conditions. As an EKF-based method, Eckenhoff et al. [26] propose a Schmidt-EKF that uses dynamic object features only for tracking the object itself, while preventing them from directly updating the ego-motion estimate, thus maintaining robustness under inaccurate target motion models. Also, Ma et al. [27] propose an IEKF framework that applies Schur complement-based marginalization to efficiently eliminate visual features from the state vector, improving scalability and accuracy. On the other hand, as a graph-based method, Song et al. [28] propose a dynamic-aware VIO system that suppresses the influence of dynamic features by adaptively reweighting their motion residuals based on Black-Rangarajan duality, without explicitly classifying or rejecting them. Song et al. [29] extend this approach by introducing Adaptive Truncated Least Squares to fully eliminate the effect of outliers and incorporate bias correction mechanisms for handling abruptly dynamic objects. Liu et al. [30] proposed IPL-SLAM, which filters dynamic features using instance segmentation and motion consistency checks while framing the fusion of semantic and geometric cues as a biologically inspired perception strategy. In these works, dynamic features are effectively excluded from ego-pose estimation through robust optimization. In natural vision, dynamic elements are not always regarded as noise but often provide informative cues for navigation. This perspective motivates the incorporation of dynamic landmarks into VO rather than discarding them.

The aforementioned methods reject outlier features detected from dynamic objects so that the effect of measured features gets lower and they will be not suitable in highly dynamic environments. Several studies have explored ways to incorporate dynamic features into ego-pose estimation rather than simply excluding them. As an EKF-based approach, Todoran et al. [31] propose an EKF-based SLAM framework that models dynamic objects as individual state variables, allowing the system to incorporate their motion into ego-pose estimation rather than discarding them as outliers. This object-level approach improves robustness in dynamic environments by leveraging stable object motion for pose correction. However, the method is sensitive to segmentation quality and association accuracy because pose updates are performed only after segments are grouped and tracked as coherent objects. As another approach of EKF-based method, Chiu [32] proposes a unified optimization-based framework that jointly estimates ego-pose, static landmarks, and dynamic object trajectories using both static and dynamic features. This approach integrates localization, mapping, and multi-object tracking within a single EKF-based backend, offering a coherent treatment of dynamic scenes. However, the method does not incorporate explicit outlier rejection, making it vulnerable to erroneous dynamic feature measurements. The evaluation is limited to simulation, and no mechanism is provided to ensure robustness under real-world perception errors. As a graph-based approach, Tian et al. [33] propose DL-SLOT, a dynamic LIDAR SLAM and object tracking framework that jointly estimates the ego-pose and the states of stationary and dynamic objects within a unified sliding window-based collaborative graph optimization. Also, Huang et al. [34] present a graph-based backend that explicitly clusters landmarks into rigid bodies, jointly estimates both ego-pose and per-object motions through decoupled factor graph optimization, and thereby mitigates the negative impact of dynamic objects on pose estimation without discarding their measurements as outliers.

Previous EKF-based methods that use dynamic landmarks for estimation did not provide a clear strategy for outlier rejection in dynamic environments, making the implementation of VO difficult in practice [31,32]. Conventional 1-point RANSAC assumes that all features belong to a static scene and thus rejects those affected by independently moving objects, resulting in the loss of informative visual cues and degraded pose estimation in dynamic environments. To address this limitation, we propose a novel EKF-based VO methodology that modifies 1-point RANSAC to detect dynamic objects and incorporate both static and dynamic landmarks into ego-pose estimation. A supplementary video shows the evaluation of the proposed method, including comparisons with classical methods (Video S1). This design is consistent with a bio-inspired perspective, where dynamic visual cues are treated as informative signals rather than discarded as outliers, reflecting adaptive strategies observed in biological perception. This paper focuses on applications for vehicle navigation with a stereo camera, and the main contributions are as follows:

(1)

A new algorithm of EKF-based visual odometry using stereo camera is proposed, which utilizes both static and dynamic landmarks for ego-pose estimation. This method uses instance segmentation to detect and track objects from image, so that it can use the characteristics of image. Also, in multiple object tracking on road, our method is robust to truncation due to angle of view, which often causes a shift in the observed point cloud centroid as the object moves near the boundary of the field of view.

(2)

Reasonable outlier rejection in dynamic environment is conducted by 1-point RANSAC and RANSAC-based transformation estimation from point cloud. The method of outlier rejection and determining whether an object is static or dynamic is proposed.

(3)

Tests are conducted using real-world image dataset to evaluate the performance of ego-pose estimation in dynamic environment with noise.

This paper is organized as follows. In Section 2, overall structure of the modified 1-point RANSAC for dynamic environments is proposed. In Section 3, evaluations are conducted to determine whether the proposed algorithm is suitable for estimating ego-pose in dynamic environments. Finally, in Section 4, conclusions are stated.

2. Modified 1-Point RANSAC for Dynamic Environments

In this section, the overall structure and the detailed methods of the proposed algorithm using both static and dynamic landmarks assuming dynamic environments is introduced. Since EKF is employed for the estimation, the state transition model and the measurement model used in the filter are first introduced. The proposed algorithm is divided into 3 steps: (1) measurement by stereo camera, (2) the classical 1-point RANSAC algorithm assuming static environment, (3) additional steps for considering dynamic landmarks. Since the proposed method incorporates the classical method, before introducing the ego-pose estimation method assuming dynamic environments, classical 1-point RANSAC algorithm which is assuming static environments is briefly introduced to define variables and facilitate comparison with the modified approach. Then, detailed methods for state estimation of dynamic objects and augmented estimation of ego-pose and landmark positions used in the proposed algorithm are further explained.

2.1. Models for EKF

In this paper, in the EKF-based VO framework, the ego-pose evolves according to the state transition model,

$${\mathsf{\xi }}_k=\mathit{g}({\mathsf{\xi }}_{k-1},{\mathit{u}}_k)+{\mathit{q}}_{\xi },$$

(1)

where ${\mathsf{\xi }}_k\triangleq {[x,y,\psi ]}_k^{\top }$ denotes the ego-pose of the vehicle in the global coordinates at current frame k, ${\mathit{u}}_k$ denotes the control input and ${\mathit{q}}_{\xi }\sim \mathcal{N}(\mathbf{0},{\mathit{Q}}_{\xi })$ denotes the state noise of the ego-pose. $\mathit{x}\triangleq {[x,y]}^{\top }$ denotes the ego position on the road plane, and $\psi$ denotes the ego heading on road plane. In this paper, the bicycle model is employed as the state transition model $\mathit{g}$.

The positions of landmarks are assumed to be static in the classic 1-point RANSAC, the state transition of the landmark position is represented as

$${\mathit{X}}_k^i={\mathit{X}}_{k-1}^{m\left(i\right)}+{\mathit{q}}_X,$$

(2)

where ${\mathit{X}}_k^i\triangleq {[X,Y]}_k^{i\top }$ denotes the position on the 2D road plane of i-th landmark at the k-th frame in the global coordinates, $m\left(i\right)$ denotes the index of the associated landmark in the previous frame, and ${\mathit{q}}_X\sim \mathcal{N}(\mathbf{0},{\mathit{Q}}_X)$ denotes the state noise of landmark position which is assumed to have Gaussian distribution.

The landmark measurements are represented in 2D polar coordinates relative to the camera coordinate system, so that the relationship between measurements and states is denoted as

$$\begin{array}{cc}\hfill & {\mathit{z}}_k^i\triangleq {[\rho ,\theta ]}_k^{i\top }=\mathit{h}({\mathit{X}}_k^i,{\mathsf{\xi }}_k)+{\mathit{r}}_{z,k}^i,\hfill \\ \hfill & {\widehat{\mathit{z}}}_k^i=\mathit{h}({\mathit{X}}_k^i,{\mathsf{\xi }}_k)=\left[\begin{array}{c}\sqrt{{(X_k^i-x_k)}^2+{(Y_k^i-y_k)}^2}\\ arctan2(Y_k^i-y_k,X_k^i-x_k)-{\psi }_k\end{array}\right],\hfill \end{array}$$

(3)

where ${\mathit{z}}_k^i$ denotes the measurement of the i-th landmark position relative to the camera coordinate system, $\rho$ is the distance from the agent, $\theta$ is the bearing angle relative to the ego heading $\psi$, and ${\mathit{r}}_{z,k}^i\sim \mathcal{N}(\mathbf{0},{\mathit{R}}_z\left({\mathit{z}}_k^i\right))$ represents the measurement noise of landmark i at the k-th frame.

The polar representation is adopted to accurately model the characteristics of the measurements obtained in the vehicle body frame and to incorporate the uncertainty originating from stereo-based feature reconstruction. Because the landmark positions are defined relative to the vehicle, their measurements are directly affected by the vehicle’s rotational noise, and such relationships are more conveniently expressed in polar coordinates than in Cartesian form. The range-dependent uncertainty of stereo-reconstructed feature points caused by disparity and pixel localization errors can be naturally incorporated into the EKF measurement covariance by expressing the measurements in polar coordinates, thereby improving the consistency of state estimation.

In this paper, the covariance of landmark measurement is not assumed to follow a Gaussian distribution with constant variance, but is instead assumed as a function of the radial distance $\rho$ and angle $\theta$ of the landmark measurement, represented as

$${\mathit{R}}_z\left({\mathit{z}}_k^i\right)\triangleq diag\left({\sigma }_{\rho }^2\left({\rho }_k^i\right),{\sigma }_{\theta }^2\left({\theta }_k^i\right)\right),$$

(4)

$$\begin{array}{c}{\sigma }_{\rho }^2\left({\rho }_k^i\right)\triangleq {\alpha }_{\rho }{\left({\rho }_k^i\right)}^{4}exp(-{\beta }_{\rho }{S}_{\Delta f,k}^i)\hfill \\ {\sigma }_{\theta }^2\left({\theta }_k^i\right)\triangleq {\alpha }_{\theta }{cos}^4\left({\theta }_k^i\right)exp(-{\beta }_{\theta }{S}_{f,k}^i),\hfill \end{array}$$

(5)

where $\alpha$ and $\beta$ denotes the constant parameters of each measurement, $S_{\Delta f}$ denotes the score of feature matching and $S_f$ denotes the score of feature detection. The variance is designed to decrease with increasing score through the use of an exponential function. In our assumption, the variance of radial distance $\rho$ is proportional to ${\rho }^4$ and the variance of angle $\theta$ is proportional to ${cos}^4\left(\theta \right)$. A detailed derivation supporting this assumption is discussed in Appendix D. The augmented state evolves according to the state transition model,

$${\mathsf{\chi }}_k=\mathit{f}({\mathsf{\chi }}_{k-1},{\mathit{u}}_k)+\mathit{q},{\mathsf{\chi }}_k\triangleq \left[\begin{array}{c}{\mathsf{\xi }}_k\\ {\mathit{X}}_k^i,\forall i\in {\mathcal{I}}_k\end{array}\right],$$

(6)

where ${\mathsf{\chi }}_k$ denotes the total state that contains ego-pose and landmark positions, $\mathit{q}$ denotes the noise of total state, and ${\mathcal{I}}_k$ denotes the set of inlier indices at k-th frame. The augmented state transition model is used for the final EKF update, denoted as

$$\mathit{f}({\mathsf{\chi }}_{k-1},{\mathit{u}}_k)=\left[\begin{array}{c}\mathit{g}({\mathsf{\xi }}_{k-1},{\mathit{u}}_k)\\ {\mathit{X}}_{k-1}^{m\left(i\right)},\forall i\in {\mathcal{I}}_k\end{array}\right].$$

(7)

2.2. Classical 1-Point RANSAC Assuming Static Environments

Figure 1 indicates the pipeline of the classical 1-point RANSAC algorithm and detailed procedure is represented in Appendix A. A random landmark ${\mathit{z}}_k^h$ is selected as sample for the hypothesis and a temporary state update is executed using only one sample. This procedure is repeated $n_{\mathrm{hyp}}$ times to find a sample which has the most consensus to maximize the number of inliers. $n_{\mathrm{hyp}}$, the number of hypothesis repetition, is determined to guarantee that at least one hypothesis is generated from an inlier with a probability p, given an estimated inlier ratio $ϵ$. The equation of $n_{\mathrm{hyp}}$ is represented as

$$n_{\mathrm{hyp}}={\displaystyle \frac{log(1-p)}{log\left(1-{(1-ϵ)}^1\right)}}.$$

(8)

During each RANSAC iteration, landmarks with innovation below the threshold are considered inliers. Specifically, after performing the temporary state update using a randomly selected landmark, the predicted measurements of all other landmarks are computed, and their innovations ${\mathsf{\nu }}_j={\mathit{z}}_k^j-{\widehat{\mathit{z}}}_k^j$ are evaluated. Each feature $j\in {\mathcal{J}}_k$, where ${\mathcal{J}}_k$ denotes the set of valid landmarks observed at time step k (see Section 2.3.1), is classified as an inlier if its innovation magnitude satisfies $\parallel {\mathsf{\nu }}_j\parallel <{\gamma }_{\mathrm{innov}}$, where ${\gamma }_{\mathrm{innov}}$ is a predefined innovation threshold. Based on each hypothesis, an inlier set ${\mathcal{I}}^h$ is constructed. If the number of inliers, $|{\mathcal{I}}^h|$, exceeds that of the best inlier set found so far, the best inlier set $\tilde{\mathcal{I}}$ and the corresponding hypothesis for the ego-pose and landmarks are updated accordingly. This process is repeated until the number of evaluated hypotheses reaches the predefined maximum $n_{\mathrm{hyp}}$.

Figure 1. Pipeline of 1-point RANSAC algorithm.

Figure 1. Pipeline of 1-point RANSAC algorithm.

The selected inlier set $\tilde{\mathcal{I}}$ is then used to perform a full EKF update as part of the high-innovation inliers recovery procedure, resulting in an updated state estimate ${\widehat{\mathsf{\chi }}}_{k|k}$ that includes both the ego-pose and the inlier landmark positions.

After this update, all remaining candidate features $i\in {\mathcal{J}}_k\setminus \tilde{\mathcal{I}}$ are re-evaluated against the updated state using the Mahalanobis distance,

$$d_i={\mathsf{\nu }}_i^{\top }{\mathit{S}}_i^{-1}{\mathsf{\nu }}_i,$$

where ${\mathsf{\nu }}_i$ is the innovation and ${\mathit{S}}_i$ the corresponding innovation covariance.

A feature i is additionally classified as an inlier if $d_i<{\gamma }_{{\chi }^2}$, where ${\gamma }_{{\chi }^2}$ is the chi-square threshold. Accordingly, the final inlier set is defined as

$${\mathcal{I}}_k=\tilde{\mathcal{I}}\cup \left\{i\in {\mathcal{J}}_k\setminus \tilde{\mathcal{I}}|d_i<{\gamma }_{{\chi }^2}\right\}.$$

(9)

2.3. Modified 1-Point RANSAC Assuming Dynamic Environments

In the proposed EKF-based framework, landmarks belonging to dynamic objects are not rejected but incorporated into the estimation process, considering the presence of dynamic environments. Figure 2 illustrates the overall structure of the proposed algorithm.

The proposed algorithm consists of five main stages, as summarized below. Each stage corresponds to the detailed procedures presented in Section 2.3.1, Section 2.3.2, Section 2.3.3, Section 2.3.4 and Section 2.3.5.

(1)

Measurement (Section 2.3.1): Landmark positions are reconstructed in the camera coordinate system from stereo image pairs. Instance segmentation is then applied to classify features into object and background regions.

(2)

Classical 1-point RANSAC (Section 2.3.2): Matched landmarks between consecutive frames are processed using the standard 1-point RANSAC to classify inliers and outliers. Outliers, as well as inliers with large innovation, are treated as potentially dynamic features. The estimated ego-pose at this stage serves as a temporary estimate for the following steps.

(3)

Object motion estimation (Section 2.3.3): The rigid-body motion of each detected object is estimated from its point cloud using a RANSAC-based Euclidean transform. A secondary Kalman filter is then applied to refine the state of each object using the estimated transformation.

(4)

Augmented state estimation (Section 2.3.4): The ego-pose and landmark states are jointly updated using inliers from both static and dynamic landmarks within the EKF framework, enabling consistent estimation under dynamic scenes.

(5)

Mapping of newly detected landmarks (Section 2.3.5): Newly observed or unmatched landmarks are initialized and incorporated into the map using the filtered ego-pose.

2.3.1. Feature Detection, Matching and Multiple Object Tracking

As visual measurements, features in pixel coordinates are detected and matched using a stereo camera. In this paper, SuperPoint [35] is employed for feature detection, and SuperGlue [36] is used for feature matching. Deep learning-based feature detection and matching are adopted to enhance feature matching reliability in outdoor environments where repetitive patterns such as windows, tiles, or crosswalks often cause correspondence errors, as the graph neural network in SuperGlue effectively leverages spatial relationships between keypoints to maintain robust matches under such conditions. The feature detector extracts keypoints from the left stereo image, and the feature matching algorithm finds correspondences with features in the right image based on those keypoints. The matched feature coordinates in the left and right images are represented as

$$\begin{array}{cc}\hfill & {\mathcal{F}}_k\triangleq {\left\{{\mathit{f}}_k^i\triangleq {[v,w]}_k^{i\top }\right\}}_{i=1}^{N_f},\hfill \\ \hfill & {\mathcal{F}}_k^{*}\triangleq {\left\{{\mathit{f}}_k^{*i}\triangleq {[v^{*},w^{*}]}_k^{i\top }\right\}}_{i=1}^{N_f},\hfill \end{array}$$

(10)

where ${\mathcal{F}}_k$ and ${\mathcal{F}}_k^{*}$ denote the sets of 2D pixel coordinates of $N_f$ matched features in the left and right stereo images, respectively. The left features with valid correspondences in the right image are also matched to the left image of previous frame, $k-1$. Two set of already calculated pixel points are matched by feature matching algorithm, in contrast to the matching with the right image, that the features are tracked to define the pixel position in the right image. The left features with valid correspondences in the right image and the previous frame are used to estimate ego-pose and the set of valid features is denoted as ${\mathcal{J}}_k$, and a set of new features with valid correspondences in the right image but unvalid correspondences in the previous frame is denoted as ${\mathcal{J}}_k^{*}$ Figure 3 indicates the procedure of the feature measurement. (Below: stereo image at current frame, Upper: stereo image at previous frame)

The 3D positions of landmarks in the camera coordinates, ${\acute{\mathit{X}}}_k^i$, are obtained via stereo triangulation for each inlier feature $i\in {\mathcal{I}}_k$. These positions are then converted to 2D polar coordinates relative to the camera, denoted as ${\mathit{z}}_k^i$, which serve as the measurement inputs to the filter. Landmarks are classified according to the objects by calculating which segmentation mask each feature point is located in. The set of landmark indices located in object o is denoted as ${\mathcal{P}}_k^o$. Also, the objects are assigned IDs and tracked using a MOT (Multiple Object Tracking) algorithm, so that each object’s ID can be matched with its corresponding instance from previous frames. YOLOv8 is used in the proposed algorithm to perform instance segmentation, and is integrated with the MOT algorithm ByteTrack [37] for object tracking.

2.3.2. 1-Point RANSAC Assuming Static Environment

The classical 1-point RANSAC, assuming static environment, is conducted using valid landmark positions $\left\{{\mathit{z}}_k^i|i\in {\mathcal{J}}_k\right\}$, while the high-innovation inliers recovery procedure is excluded. The best inlier set $\tilde{\mathcal{I}}$ is considered directly as the final inlier set ${\mathcal{I}}_k$. The temporarily filtered ego-pose ${\widehat{\mathsf{\xi }}}_{k|k}^{-}$ is calculated, and landmarks are divided into inliers and outliers. Unlike the outliers are just rejected in classic method, in the proposed method, they are considered potentially dynamic landmarks which have the opportunity to be used in the estimation.

When an object is considered to be potentially dynamic, it is assumed to have either outlier landmarks or landmarks that pass the innovation threshold but still exhibit motion. Since some dynamic landmarks can be mistakenly included as inliers by 1-point RANSAC due to their small innovation, the distance between each landmark’s current filtered position and its previous position is computed to detect subtle motion. Whether a landmark is considered potentially dynamic depends on whether the object it belongs to is determined to be dynamic.

To identify such cases, a subset of landmarks that passed the innovation threshold ${\gamma }_{\mathrm{innov}}$ but exhibit a significant positional change is extracted for each object. This set is defined as

$${\mathcal{D}}_{1,k}^o=\left\{i\in {\mathcal{I}}_k\cap {\mathcal{P}}_k^o|∥{\widehat{\mathit{X}}}_{k|k}^i-{\widehat{\mathit{X}}}_{k|k-1}^i∥>{\gamma }_{\mathrm{inliers}}\right\},$$

(11)

where ${\mathcal{I}}_k$ denotes the inlier set and ${\mathcal{P}}_k^o$ denotes the set of landmarks belonging to object o.

If the proportion of such landmarks for object o exceeds a predefined threshold, the object is classified as dynamic. The set of such objects is denoted as

$${\mathcal{O}}_{1,k}=\left\{o|{\displaystyle \frac{|{\mathcal{D}}_{1,k}^o|}{|{\mathcal{P}}_k^o|}}>{\gamma }_{\mathrm{ratio}}\right\}.$$

(12)

Next, landmarks that were rejected as outliers by the 1-point RANSAC procedure and failed the innovation threshold test are grouped per object, denoted as

$${\mathcal{D}}_{2,k}^o=({\mathcal{J}}_k\setminus {\mathcal{I}}_k)\cap {\mathcal{P}}_k^o.$$

(13)

An object is also considered potentially dynamic if the ratio of outlier landmarks exceeds the threshold:

$${\mathcal{O}}_{2,k}=\left\{o|{\displaystyle \frac{|{\mathcal{D}}_{2,k}^o|}{|{\mathcal{P}}_k^o|}}>{\gamma }_{\mathrm{ratio}}\right\}.$$

(14)

The total set of potentially dynamic objects is defined as the union:

$${\tilde{\mathcal{O}}}_k={\mathcal{O}}_{1,k}\cup {\mathcal{O}}_{2,k}.$$

For each object $o\in {\tilde{\mathcal{O}}}_k$, the complete set of associated dynamic landmarks is given by:

$${\tilde{\mathcal{D}}}_k^o={\mathcal{D}}_{1,k}^o\cup {\mathcal{D}}_{2,k}^o.$$

2.3.3. Estimation of Planar Motion of Dynamic Objects with Kalman Filter

Another Kalman filter is applied to each object o in the potentially dynamic object set ${\tilde{\mathcal{O}}}_k$, in order to estimate the motions of dynamic objects. The Euclidean transformation of each object, consisting of the rotation ${\Omega }_k^o\in \mathrm{SO}\left(2\right)$ and the translation ${\mathsf{\tau }}_k^o\in {\mathbb{R}}^2$, is estimated by comparing the current point cloud with the previous one. The term ${\Omega }_k^o$ denotes the rotation, and ${\mathsf{\tau }}_k^o$ denotes the translation of object o at frame k.

The estimation of transformation by comparison is conducted using raw point cloud for both current frame and previous frame, because the point cloud containing filtered landmarks is hard to estimate exact transformation. The position of each raw point at current frame in global coordinates is computed using the temporarily filtered ego-pose, denoted as

$${\tilde{\mathit{X}}}_k^i=\mathbf{\Phi }\left({\widehat{\psi }}_{k|k}^{-}\right){\acute{\mathit{X}}}_k^i+{\widehat{\mathit{x}}}_{k|k}^{-}.$$

(15)

Similarly, the position of each raw point at previous frame in global coordinates is computed using the finally filtered ego-pose at previous frame, denoted as

$${\tilde{\mathit{X}}}_{k-1}^i=\mathbf{\Phi }\left({\widehat{\psi }}_{k-1|k-1}\right){\acute{\mathit{X}}}_{k-1}^{m\left(i\right)}+{\widehat{\mathit{x}}}_{k-1|k-1},$$

(16)

where $\mathbf{\Phi }\left(\psi \right)\in \mathrm{SO}\left(2\right)$ denotes the rotation matrix of angle $\psi$.

When estimating the transformation of each point cloud, RANSAC is conducted to distinguish the inlier landmarks that fit with the estimated transform and the outliers that do not. The algorithm of RANSAC-based Euclidean transform estimation is indicated in Appendix B. As outlined in Algorithm A2, the method iteratively selects minimal pairs of point correspondences and estimates a candidate rigid trasformation using a least-squares solution as represented in Algorithm A3. For each hypothesis, the transformation is applied to all source points, and the reprojection error is computed. Inliers are determined by thresholding the error, and the transformation yielding the largest inlier set is selected as the final result. This procedure provides robustness against outliers and ensures that the estimated transformation best alignes the two point sets under Euclidean constraints.

Due to the possibility that instance segmentation may erroneously assign background pixels to object instances, the resulting point clouds can contain spurious points. To address this, additional filtering using point cloud clustering is performed to suppress such noise. DBSCAN (Density-Based Spatial Clustering of Applications with Noise) [38] is conducted to cluster the inliers of potentially dynamic point clouds. After performing point cloud clustering, the cluster containing the largest number of points is considered the representative cluster, while the remaining clusters are treated as noise and excluded from further computations. The set of representative landmark indices for each object o are denoted as ${\mathcal{D}}_k^o$.

The measurement used in Kalman filter for object state, ${\mathsf{\zeta }}_k^o$, is calculated using the estimated transformation, represented in Appendix C. While the center of the point cloud is primarily assumed to correspond to the object center, velocity and angular velocity are additionally considered to compensate for potential center shifts induced by occlusion.

The measurement of object state is denoted as

$${\mathsf{\zeta }}_k^o=\left[\begin{array}{c}{\displaystyle \frac{1}{\left|{\tilde{\mathcal{D}}}_k^o\right|}}\sum _{i\in {\tilde{\mathcal{D}}}_k^o}{\tilde{\mathit{X}}}_k^i\\ {\mathsf{\tau }}_o/\Delta t\\ arctan2(\Omega {(2,1)}_o,\Omega {(1,1)}_o)/\Delta t\end{array}\right].$$

(17)

To ensure the reliability of object motion estimation, several validation steps are incorporated before accepting the estimated state as a valid measurement for the Kalman filter update. These checks are essential to eliminate ill-posed or noisy results that may compromise the accuracy of the estimation process. First, each object must be associated with at least three 3D points to enable rigid transformation estimation. Then, the rigid transformation matrix $\mathit{M}$ between the previous and current point sets is estimated using a RANSAC-based method. The transformation is accepted only if it yields a non-empty set of inliers that support the estimated motion. From the estimated rigid transform $\mathit{M}$, the translation vector $\mathsf{\tau }$ and rotation matrix $\Omega$ are extracted. Third, the spatial distribution of the inlier points is evaluated using their covariance matrix. If the maximum eigenvalue of this matrix exceeds a threshold $T_{\mathrm{cov}}$, indicating high spatial dispersion or unreliability, the object is discarded from further processing. An object is classified as an outlier and excluded from the Kalman filter update if it fails to satisfy any of the above criteria.

And the measurement model is represented as

$${\mathsf{\zeta }}_k^o={\mathit{s}}_k^o+{\mathit{r}}_{\zeta ,k}^o$$

(18)

where ${\mathit{s}}_k^o\triangleq {[\mathit{c},\dot{\mathit{c}},\dot{\varphi }]}_k^{o\top }$ denotes the state, ${\mathit{c}}_k^o\triangleq {[\overline{X},\overline{Y}]}_k^{o\top }$ denotes the position, ${\dot{\varphi }}_k^o$ denotes the angular velocity of the point cloud of object o at k-th frame, and ${\mathit{r}}_{\zeta ,k}^o\sim \mathcal{N}\left(\mathbf{0},{\mathit{R}}_{\zeta }\left({\mathsf{\zeta }}_k^o\right)\right)$ denotes the measurement noise of object state.

The measurement error covariance ${\mathit{R}}_{\zeta }\left({\mathsf{\zeta }}_k^o\right)$ is calculated considering the number of point used to compute each value of the state, represented as

$${\mathit{R}}_{\zeta }\left({\mathsf{\zeta }}_k^o\right)={\displaystyle \frac{{\sigma }^2}{|{\tilde{\mathcal{D}}}_k|}}·\mathrm{diag}\left({\mu }_{\mathrm{pos}},{\mu }_{\mathrm{pos}},{\mu }_{\mathrm{vel}},{\mu }_{\mathrm{vel}},{\mu }_{\mathrm{angle}}\right)$$

(19)

Each object state is filtered assuming planar motion with constant velocity, constant anglular velocity, represented as

$${\mathit{s}}_k^o=\mathit{A}{\mathit{s}}_{k-1}^o+{\mathit{q}}_s,\mathit{A}=\left[\begin{array}{ccccc}1& 0& \Delta t& 0& 0\\ 0& 1& 0& \Delta t& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 1\end{array}\right]$$

(20)

where ${\mathit{q}}_s\sim \mathcal{N}(\mathbf{0},\Lambda )$ denotes the state error of objects.

2.3.4. Augmented Estimation with Both Static and Dynamic Landmarks

Since the motions of the dynamic objects are estimated, both static and dynamic landmarks are ready to be used in estimation. Unlike the transition model of the static landmarks is same as mentioned in Equation (2), the transition model of each dynamic landmark is denoted as

$${\mathit{X}}_k^i=\mathit{l}({\mathit{X}}_{k-1}^{m\left(i\right)},{\mathit{c}}_{k-1}^o,{\dot{\mathit{c}}}_k^o,{\dot{\varphi }}_k^o)+{\mathsf{\eta }}_k^i,$$

(21)

$$\begin{array}{c}\mathit{l}({\mathit{X}}_{k-1}^{m\left(i\right)},{\mathit{c}}_{k-1}^o,{\dot{\mathit{c}}}_k^o,{\dot{\varphi }}_k^o)\hfill \\ \triangleq \Phi ({\dot{\varphi }}_k^o\Delta t)({\mathit{X}}_{k-1}^{m\left(i\right)}-{\mathit{c}}_{k-1}^o)+{\mathit{c}}_{k-1}^o+{\dot{\mathit{c}}}_k^o\Delta t,\hfill \end{array}$$

(22)

where $\Phi ({\dot{\varphi }}_k^o\Delta t)\in \mathrm{SO}\left(2\right)$ denotes the rotation matrix of angle ${\dot{\varphi }}_k^o\Delta t$ and ${\mathsf{\eta }}^i$ denotes the state noise of landmark i.

When using dynamic landmarks to estimate ego-pose, it can cause more error than assuming static environment if there exists large error in object state estimation. To avoid the adverse effect of using the proposed algorithm, the vailidity of the estimation of the dynamic object states should be judged. Mean innovation of the estimated landmark positions of each object is calculated to judge the validity of each object denoted as

$${\overline{\nu }}_{1,k}^o={\displaystyle \frac{1}{|{\tilde{\mathcal{D}}}_k^o|}}\sum _{i\in {\mathcal{O}}_k^o}\left({\tilde{\mathit{X}}}_k^i-\mathit{l}({\widehat{\mathit{X}}}_{k-1|k-1}^{m\left(i\right)},{\mathit{c}}_{k-1}^o,{\dot{\mathit{c}}}_k^o,{\dot{\varphi }}_k^o)\right)$$

(23)

$${\overline{\nu }}_{2,k}^o={\displaystyle \frac{1}{|{\tilde{\mathcal{D}}}_k^o|}}\sum _{i\in {\tilde{\mathcal{O}}}_k^o}\left({\tilde{\mathit{X}}}_k^i-{\widehat{\mathit{X}}}_{k-1|k-1}^{m\left(i\right)}\right)$$

(24)

$${\mathcal{O}}_k=\left\{o\in {\tilde{\mathcal{O}}}_k|{\overline{\nu }}_{1,k}^o<{\overline{\nu }}_{2,k}^o\right\}$$

(25)

The total transition model is represented as

$$\begin{array}{c}\mathit{f}({\mathsf{\chi }}_{k-1},{\mathit{u}}_k)=\hfill \\ \left[\begin{array}{c}\mathit{g}({\mathsf{\xi }}_{k-1},{\mathit{u}}_k)\\ \begin{array}{c}\forall i\in {\mathcal{I}}_k\cup \bigcup _{o\in {\mathcal{O}}_k}{\mathcal{D}}_k^o,\hfill \\ \left\{\begin{array}{cc}{\mathit{X}}_{k-1}^{m\left(i\right)}\hfill & \mathrm{if}i\in {\mathcal{I}}_k\setminus \bigcup _{o\in {\mathcal{O}}_k}{\mathcal{D}}_k^o\hfill \\ \mathit{l}({\mathit{X}}_{k-1}^{m\left(i\right)},{\mathit{c}}_{k-1}^o,{\dot{\mathit{c}}}_k^o,{\dot{\varphi }}_k^o)\hfill & \mathrm{if}i\in {\mathcal{D}}_k^o,o\in {\mathcal{O}}_k\hfill \end{array}\right.\hfill \end{array}\end{array}\right]\hfill \end{array}$$

(26)

The state transition model $\mathit{f}({\mathsf{\chi }}_{k-1},{\mathit{u}}_k)$ jointly updates the ego-pose and the landmark positions as components of an augmented state vector. The ego-pose is propagated using the motion model $\mathit{g}({\mathsf{\xi }}_{k-1},{\mathit{u}}_k)$, while the landmarks are updated based on whether they are associated with dynamic objects or not. Dynamic landmarks are propagated according to the estimated motion of their corresponding object, whereas static ones retain their previous positions.

The Jacobian of the total transition model is also represented as

$$\begin{array}{c}\mathit{F}({\mathsf{\chi }}_{k-1},{\mathit{u}}_k)=\hfill \\ \mathrm{blockdiag}\left(\mathit{G}({\mathsf{\xi }}_{k-1},{\mathit{u}}_k),\begin{array}{c}\forall i\in {\mathcal{I}}_k\cup \bigcup _{o\in {\mathcal{O}}_k}{\mathcal{D}}_k^o,\hfill \\ \left\{\begin{array}{cc}\mathit{I}\hfill & \mathrm{if}i\in {\mathcal{I}}_k\setminus \bigcup _{o\in {\mathcal{O}}_k}{\mathcal{D}}_k^o\hfill \\ \Phi ({\dot{\varphi }}_k^o\Delta t)\hfill & \mathrm{if}i\in {\mathcal{D}}_k^o,o\in {\mathcal{O}}_k\hfill \end{array}\right.\hfill \end{array}\right)\hfill \end{array}$$

(27)

The Jacobian of the state transition model, $\mathit{F}({\mathsf{\chi }}_{k-1},{\mathit{u}}_k)$, is represented in block-diagonal form. It consists of the ego-pose Jacobian $\mathit{G}({\mathsf{\xi }}_{k-1},{\mathit{u}}_k)$ and landmark-specific blocks, where each block is set to the identity matrix for static landmarks, and to a rotation matrix $\Phi ({\dot{\varphi }}_k^o\Delta t)$ for dynamic landmarks, depending on whether $i\in {\mathcal{D}}_k^o$ for some $o\in {\mathcal{O}}_k$.

The update of total state is updated following the standard EKF procedures, represented as

(1)

State Prediction ${\widehat{\mathsf{\chi }}}_{k|k-1}=\mathit{f}({\widehat{\mathsf{\chi }}}_{k-1|k-1},{\mathit{u}}_k)$

(2)

Covariance Prediction ${\mathit{P}}_{k|k-1}={\mathit{F}}_k{\mathit{P}}_{k-1|k-1}{\mathit{F}}_k^{\top }+{\mathit{Q}}_k$

(3)

Residual ${\mathsf{\nu }}_k={\mathit{z}}_k-h\left({\widehat{\mathit{x}}}_{k|k-1}\right)$

(4)

Innovation Covariance ${\mathit{S}}_k={\mathit{H}}_k{\mathit{P}}_{k|k-1}{\mathit{H}}_k^{\top }+{\mathit{R}}_k$

(5)

Kalman Gain ${\mathit{K}}_k={\mathit{P}}_{k|k-1}{\mathit{H}}_k^{\top }{\mathit{S}}_k^{-1}$

(6)

State Update ${\widehat{\mathsf{\chi }}}_{k|k}={\widehat{\mathsf{\chi }}}_{k|k-1}+{\mathit{K}}_k{\mathsf{\nu }}_k$

(7)

Covariance Update ${\mathit{P}}_{k|k}=(\mathit{I}-{\mathit{K}}_k{\mathit{H}}_k){\mathit{P}}_{k|k-1}$

2.3.5. Mapping of Newly Detected Landmarks

Mapping of newly detected landmarks, $\left\{{\acute{\mathit{X}}}_k^i|i\in {\mathcal{J}}_k^{*}\right\}$, is conducted in final sequence. The positions of the newly detected landmarks is calculated using ego-pose state, denoted as

$${\tilde{\mathit{X}}}_k^i=\Phi \left({\widehat{\psi }}_{k|k}\right){\acute{\mathit{X}}}_k^i+{\widehat{\mathit{x}}}_{k|k}$$

(28)

Mapping of newly detected landmarks, $\left\{{\acute{\mathit{X}}}_k^i|i\in {\mathcal{J}}_k^{*}\right\}$, is conducted at the final time step. Each landmark position, originally represented in the camera coordinate system, is transformed into the global coordinate frame using the estimated ego-pose:

$${\tilde{\mathit{X}}}_k^i=\Phi \left({\widehat{\psi }}_{k|k}\right){\acute{\mathit{X}}}_k^i+{\widehat{\mathit{x}}}_{k|k},$$

(29)

where ${\tilde{\mathit{X}}}_k^i$ denotes the position of landmark i in the global coordinate system.

Then, the states of both static and dynamic objects are calculated by newly detected landmark positions as denoted in Equation (17).

3. Evaluation

3.1. Environment Settings

The evaluation is conducted to compare the proposed algorithm to classical 1-point RANSAC algorithm. KITTI odometry dataset [39] is used to evaluate the performance of the classic 1-point RANSAC assuming static environments and modified 1-point RANSAC assuming dynamic environment. The test is conducted under the assumption that the state transition model follows a bicycle model, considering two configurations: (1) incorporating the control inputs into the augmented state vector for joint estimation, and (2) treating them as measurements. In the first configuration, the control inputs are included in the augmented state vector, which is denoted as ${\mathsf{\xi }}_k\triangleq {[x,y,\psi ,V,\delta ]}_k^{\top }$. In the second configuration, control inputs are treated as measurements. The control input vector ${\mathit{u}}_k\triangleq {[V,\delta ]}_k^{\top }$ consists the longitudinal velocity V and the steering angle $\delta$ of the vehicle. Gaussian noise is also added to the velocity and steering angle to model sensor uncertainty for the test. The test is conducted for 200 frames of 11 sequences. Among the 11 sequences, sequences 1, 4, 8 consist dynamic objects and other sequences do not consist dynamic objects.

To evaluate the local accuracy of the proposed VO method, we adopt the Relative Pose Error (RPE) as an evaluation metric. The RPE quantifies the discrepancy between the estimated and ground-truth relative motions by comparing frame-to-frame transformations.

All parameters, including the EKF process and measurement noise covariances, RANSAC thresholds, and segmentation settings, were fixed across all experiments to ensure fair comparison. Only the initial state of the ego-pose was changed depending on the starting frame of each sequence.

3.2. Evaluation Using an Augmented State Vector Including Control Inputs

Evaluation is first conducted using an augmented state vector including control inputs. Sequence 4, which was collected on a wide highway with dynamic vehicles, is presented as a representative example that demonstrates the effectiveness of the proposed method in dynamic environments. Figure 4 represents the comparison of the estimation results when control inputs are estimated as states (middle) and when they are used as measurements (bottom). The ego-pose is represented by a circle without an outline, indicating the agent’s position, and a short line, indicating its orientation. The agent’s trajectory is depicted as a line following its motion. Landmarks are plotted as small dots: inliers are shown as opaque points, while outliers are shown as translucent ones. In addition, the raw landmarks of dynamic objects are represented as squares. The distinction between objects is expressed through color differences in the landmarks. The positions of objects are indicated by circles, and the velocities of dynamic objects are visualized using arrows.

The scene includes one vehicle approaching the agent (green), another moving away from it (purple), and static landmarks distributed throughout the environment (brown). Since only a few static landmarks are available and they are difficult to match, assuming a static environment and applying the classical method can result in significant estimation errors or even lead to divergence. Although the absence of known control inputs during estimation makes the estimation highly sensitive to noise, the proposed method produces a coarse but informative estimate of object states that helps reduce ego-pose errors during estimation.

Figure 5 shows the estimated trajectory. Although the proposed method is not free from estimation errors, it demonstrates significantly better robustness compared to the classical approach, which suffers from severe scale drift and larger error in orientation estimation. This result confirms that incorporating the motion of dynamic objects into the estimation process helps compensate for ego-pose errors.

Figure 6 shows the estimated ego-pose state values, where the proposed method exhibits less perturbation in estimation of orientation and less scale drift.

Table 1. Relative Pose Error on KITTI sequences with control inputs estimated as states.

Sequence	Classical	Proposed
00	0.2754	0.2130
01	1.8018	0.8112
02	0.3248	0.4334
03	0.2791	0.3508
04	0.9288	0.3193
05	0.3511	0.3559
06	0.1342	0.1094
07	1.0964	0.2852
08	0.6455	0.4526
09	0.5847	0.4866
10	0.2120	0.5997
Mean	0.6031	0.4016
Dynamic cases	1.1254	0.5277

and Figure 7 further confirm the effectiveness of the proposed method. A substantial reduction in RPE is observed in dynamic sequences, achieving up to 53% improvement over the classical method. While performance in static environments is mostly comparable, the proposed method demonstrates strong robustness in handling dynamic objects through joint estimation of control inputs and states.

3.3. Evaluation Using Control Inputs as Measurements

The evaluation was also conducted by treating the control inputs as measurements. Figure 8 presents the estimation results of six selected frames from sequence 8.

For each frame, the upper figure shows the result of image-based measurement, including the detected features and instance segmentation masks. The lower figure shows the result of estimation.

Beginning with Frame 16 (Figure 8a), a dynamic object (pink) is present and is moving away from the agent. It is shown that the state of this moving object is successfully estimated.

In Frame 27 (Figure 8b), the dynamic object detected in Frame 16 remains dynamic in reality but is considered an outlier due to a large innovation, caused by having few landmarks and being too far from the agent. Instead, another dynamic object (green) appears, which is approaching the agent, and its state is also correctly estimated.

This dynamic object is consistently tracked in the subsequent frames. In Frames 39 (Figure 8d) and 42 (Figure 8e), the object state is shown to be estimated with minimal center shift caused by truncation during the filtering process.

Finally, in Frame 44 (Figure 8f), when the object is no longer visible in the image, its landmarks are excluded from the ego-pose estimation.

Figure 9 represents the estimated trajectories by classical 1-point RANSAC and the proposed method and Figure 9 represents the ego-pose values throughout time. Figure 9 show that the trajectory estimated by the proposed method follows the ground truth more closely than that of classical 1-point RANSAC. In Figure 9, bigger error is observed when using classical 1-point RANSAC than proposed algorithm for frames that exist dynamic objects. Figure 10 shows the yaw angle of the agent at dynamic frames in detail. It is observed that the yaw angle is estimated with less perturbation when used the proposed method.

The evaluation result of the proposed algorithm is presented in

Table 2. Relative Pose Error on KITTI sequences with control inputs as measurements.

Sequence	Classical	Proposed
00	0.0160	0.0173
01	0.0273	0.0214
02	0.0170	0.0166
03	0.0167	0.0144
04	0.0257	0.0187
05	0.0145	0.0133
06	0.0146	0.0160
07	0.0142	0.0112
08	0.0220	0.0202
09	0.0135	0.0149
10	0.0153	0.0157
Mean	0.0179	0.0163
Dynamic cases	0.0250	0.0201

and Figure 11. In sequences 1, 4, 8, that contain dynamic objects, lower relative error is obtained with the proposed algorithm. There is slight variations in error in static environments, but the overall performance is nearly identical. 9% lower error is obtained for total sequences, and 20% lower error is obtained for sequences of dynamic environments. There are some factors that improve performance and others that degrade it at static environments. Additional filtering procedure that reject high-innovation landmarks can contribute to the performance of the estimation. In contrast, static objects can be misclassified as dynamic objects by the noise of the measurement, and it can cause decrease in performance of the estimation.  

4. Conclusions

We proposed a novel EKF-based visual odometry method designed for dynamic environments, where dynamic landmarks are explicitly incorporated into the ego-pose estimation process. Unlike conventional approaches that discard or ignore dynamic elements, our method leverages a 1-point RANSAC-based EKF framework, enabling robust state estimation even in the presence of moving objects and noisy measurements.

The proposed method demonstrated consistent improvements in relative pose error across various dynamic scenarios, validating its effectiveness in maintaining accurate ego-pose estimation under real-world conditions. We showed that the algorithm can estimate and track both ego-motion and object states without significant degradation caused by occlusion, landmark sparsity, or truncation artifacts.

Moreover, the framework is computationally efficient, achieving an average computation time of 0.0303 s per frame for the Kalman filter and 0.158 s per frame for image processing (feature matching and instance segmentation). This makes the system suitable for real-time applications and practical deployment in autonomous navigation tasks. The proposed method can be extended to various mobile platforms such as autonomous vehicles, drones, or robots operating in dynamic environments, where the assumption of static surroundings frequently fails.

While the method demonstrates reliable performance in typical dynamic scenarios, it may face challenges in extremely highly dynamic scenes or when measurements are severely degraded by noise. As part of future work, we plan to enhance the filter’s robustness in such conditions by incorporating motion priors and adaptive noise models that can better handle large landmark uncertainty and fast object motion. These improvements are expected to further increase the accuracy and reliability of ego-pose estimation in complex real-world environments.