State estimation method and system under heading constraint

Abstract

The invention provides a state estimation method and system under heading constraint. The method comprises the following steps: under the condition that heading is known, augmenting a state vector anda covariance matrix by utilizing nodal increment of a constraint straight line, and augmenting an original state equation; calculating an initial value; constructing pseudo measurement based on a position constraint relation and a corresponding speed constraint relation; augmenting a measurement equation by utilizing the pseudo measurement; based on the augmented state equation and the augmentedmeasurement equation, and initial values of the state vector and the covariance matrix, carrying out extended Kalman filtering to obtain a filtering result of a target position. By adopting the stateestimation method and system under the heading constraint, the state estimation problem under incomplete constraint conditions is solved; prior known information is sufficiently utilized and wastes ofthe information are avoided.

Description

technical field [0001] The invention relates to the technical field of target tracking, in particular to a state estimation method and system under heading constraints. Background technique [0002] When estimating the state that satisfies the constraints, there is the problem of incomplete constraint information. For example, in a multi-lane vehicle driving along a road, the heading of the target is constrained to be in the direction of the straight road, which implies the heading constraint prior information, however, the vehicle's running track is unknown a priori. When the traditional filtering method solves the estimation problem, (1) the constraint information is not considered, resulting in waste of information; (2) the constraint information is required to be known, which cannot solve the constraint estimation problem with fuzzy or partially known constraint information. Therefore, it is of practical significance to provide a method that reasonably utilizes incomple...

AI Technical Summary

Problems solved by technology

[0010] The technical problem to be solved by the present invention is to propose a method and system for state estimation under heading constraints to solve the intercept information on the constrained straight line and determine the constrained straight line of the target state, aiming at the problem that the existing methods cannot solve the estimation problem of incomplete constraints , and combined with the optimized filtering technique to improve the constraint state estimation accuracy

Images