What is the best way to estimate the state
[x-position;
y-position;
heading (yaw angle);
velocity;
acceleration;
curvature (or yaw rate)]
of a moving leading vehicle with sensors mounted on a follower/ego vehicle?
The following measurements of the leading vehicle are obtained via radar sensors mounted on the ego vehicle.
- x-y-position in the ego coordinate frame
- heading in ego coordinates
- relative and absolute velocity and acceleration
No information about curvature (yaw rate). This should be estimated which is possible using the lateral acceleration and longitudinal velocity.
For estimation I think of using EKF or nonlinear moving horizon estimation.
Considering that no prediction about the moving leader vehicle can be made because the control inputs are unknown. Only the measurments (update step) and the movements of the ego vehicle are available (incroporate in prediction).
What kind of model would be appropriate for the whole scenario?
- just a model for the leading vehicle? (e.g. bicycle model)
- just a model for the following ego vehicle?
- or a combination of both vehicles?
Option 1 would be perfect in combination with a simple bicycle model if there was an outside observer who is not moving. Option 2 is not really an option because the configuration of the leader vehicle should be estimated. Option 3 seems to me the right way because of the following thoughts:
Looking from the ego coordinates: is it correct correct that a motion change of the ego vehicle will seem as if the measured locations of the leading vehicle changed? If so will I need a coordinate transformation or is it better to use a model in global coordinates, then transform the measurements (which are in ego coordinates)?. The approach using global coordinates seems counter intuitive because the final estimate should be used for the follower/ego vehicle as reference trajectory.
Can you give me a hint which coordinate frame (global or ego) to use, which model to use in the prediction step and if my thoughts on motion changes are correct?
Or do you know any sources that address this or a similar issue?
For the process model I thought of
x_k+1 = x_k + v_k * cos(heading_k)
y_k+1 = y_k + v_k * sin(heading_k)
heading_k+1 = heading_k
v_k+1 = v_k
a_k+1 = a_k
curvature_k+1 = curvature_k + a_y_k/v_x_k
of course there should be some process noise added, especially for heading, velocity acceleration and curvature because these measurements are rather inaccurate.
For heading, velocity and acceleration I would use the previous estimates (or measurements) since no other source except the measurements from the sensors are available.
Curvature is computed using the acceleration in (global?) y-direciton and the (global?) velocity in x-direction curvature = a_y/v_x
The measurement model looks probably something like this:
y = [1 1 1 1 1 0;
0 1 0 0 0 0;
0 0 1 0 0 0;
0 0 0 1 0 0;
0 0 0 0 1 0;
0 0 0 0 0 0]*x
where x is the state vector [x-position; y-position; heading (yaw angle); velocity; acceleration; curvature or (yaw rate)]
The trajectory must be smooth and should be exactly like the driven path of the leader vehicle. So I think some sort of estimation will be necesseray in order to avoid following the measurements in straight lines, which would lead to an unsteady trajectory.
The mesurements of the heading, velocity and acceleration have a rather high variance depending on the situation.
