An Iterative Method Based on the Marginalized Particle Filter for Nonlinear B-Spline Data Approximation and Trajectory Optimization

Abstract

The B-spline function representation is commonly used for data approximation and trajectory definition, but filter-based methods for NWLS approximation are restricted to a bounded definition range. We present an algorithm termed NRBA for an iterative NWLS approximation of an unbounded set of data points by a B-spline function. NRBA is based on a MPF, in which a KF solves the linear subproblem optimally while a PF deals with nonlinear approximation goals. NRBA can adjust the bounded definition range of the approximating B-spline function during run-time such that, regardless of the initially chosen definition range, all data points can be processed. In numerical experiments, NRBA achieves approximation results close to those of the Levenberg–Marquardt algorithm. An NWLS approximation problem is a nonlinear optimization problem. The direct trajectory optimization approach also leads to a nonlinear problem. The computational effort of most solution methods grows exponentially with the trajectory length. We demonstrate how NRBA can be applied for a multiobjective trajectory optimization for a BEV in order to determine an energy-efficient velocity trajectory. With NRBA, the effort increases only linearly with the processed data points and the trajectory length.