A Survey of Code Optimization Methods for Kalman Filter Extensions <s> Optimization of Iterative KFEs <s> Nonlinear filters are often very computationally expensive and usually not suitable for real-time applications. Real-time navigation algorithms are typically based on linear estimators, such as the extended Kalman filter (EKF) and, to a much lesser extent, the unscented Kalman filter. This work proposes a novel nonlinear estimator whose additional computational cost is comparable to (N-1) EKF updates, where N is the number of recursions, a tuning parameter. The higher N the less the filter relies on the linearization assumption. A second algorithm is proposed with a differential update, which is equivalent to the recursive update as N tends to infinity. <s> BIB001 </s> A Survey of Code Optimization Methods for Kalman Filter Extensions <s> Optimization of Iterative KFEs <s> In this study, the authors focus on improving measurement update of existing nonlinear Kalman approximation filter and propose a new sigma-point Kalman filter with recursive measurement update. Statistical linearization technique based on sigma transformation is utilized in the proposed filter to linearize the nonlinear measurement function, and linear measurement update is applied gradually and repeatedly based on the statistically linearized measurement equation. The total measurement update of the proposed filter is nonlinear, and the proposed filter can extract state information from nonlinear measurement better than existing nonlinear filters. Simulation results show that the proposed method has higher estimation accuracy than existing methods. <s> BIB002
Optimizations presented in sections 5.1 and 5.2 are most widely used in the field of Simultaneous Localization and Mapping (SLAM). In SLAM, the state contains a dynamic part that represents the agent (robot) that is mapping static landmarks in the environment and localizing itself. The landmarks are the static part of the state. We propose that these optimizations can be applied also to KFEs that do the update iteratively e.g. Recursive Update Filter (RUF) BIB001 and its extension for sigma-point filters BIB002 or posterior linearization filters  . In RUF, the state is updated by applying the update in parts that have smaller impact than the original update. After each part of the update the linearization is recomputed. These iterative filters are developed assuming additive Gaussian noise. Instead of making the updates for full state the update could be done iteratively only for the observed variables. Because the iterative update is actually one update, the unobserved state variables are static during the partial updates. In this example we consider a state model with 3D position, velocity, and acceleration i.e. 9 state variables. Measurements used are range measurements from 3 base stations. The measurement model for range from the ith base station is where r i is the location of the ith base station. In our example, the prior has a large variance and its mean is located so that the linearization about the mean is not accurate. Figure 2 shows the example situation and the posterior means which are computed with different number of iterations of RUF. The estimate with one iteration (T = 1) is identical to EKF estimate and is not close to the true location. The estimate with 10 iterations is close to the true location. The computation of 10 iterations with RUF involves the updates to the 9 × 9 state covariance matrices 10 times. Because the measurement model (81) depends only on 3 first state variables the active state can be selected to consist of only those and the iterative update is computed only for the position variables. The full covariance update is computed after all iterations are applied. In this update scheme, the 3 × 3 covariance matrix is updated T times and the 9 × 9 matrix is updated only once using (54)-(57).