What is the best way to estimate the state

heading (yaw angle); 
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.

  • $\begingroup$ You don't mention what sensors are on the ego vehicle for any inertial reference. Are there any? As a side note you may want to rethink your statement that no prediction of the leader vehicle can be made. Between inertia and most ground contact conditions it seems like you can predict a lot. More if you know the steering kinematics. $\endgroup$
    – hauptmech
    Sep 12, 2016 at 1:03
  • $\begingroup$ Thanks for your reply. The ego vehicle is fully aware of its state using odometry, imu,... and radar sensors to track the leader vehicle. $\endgroup$
    – evolved
    Sep 12, 2016 at 1:37
  • $\begingroup$ I know that I can make predicitons using e.g. the simple bicycle model for the leading vehicle. Unfortunately I wouldn't know any steering kinematics. Can you give me a hint on which other model could be useful? $\endgroup$
    – evolved
    Sep 12, 2016 at 1:43
  • $\begingroup$ There is no physical connection between the leader and ego correct? The path of the leader vehicle is unknown and the path of the follower is under your control? This is my understanding so far. $\endgroup$
    – hauptmech
    Sep 12, 2016 at 3:52
  • 1
    $\begingroup$ @user76646 I posted an answer which describes my current approach. $\endgroup$
    – evolved
    Oct 4, 2016 at 19:29

2 Answers 2


It sounds like you can estimate everything you need to with a direct calculation of the transform between each vehicle since that is provided for you. For the derivative parameters, if your sensors give direct measurement (velocity from doppler on the radar for instance) you probably want to use those values rather than filtering.

Without knowing more I'd be a bit wary of the heading measurement. For instance, is it a direct measurement or an estimate from the velocity info?

The EKF seems unnecessary.

  • $\begingroup$ Soory I was a bit vague on the trajectory requirements. It must be a smooth trajectory, which 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. $\endgroup$
    – evolved
    Sep 12, 2016 at 2:48
  • $\begingroup$ The heading the velocity as well as the acceleration have a high variance depending on the current situation. $\endgroup$
    – evolved
    Sep 12, 2016 at 2:49

My current approach is the following:

  1. transform the the noisy leader vehicle state from ego coordinates to global coordinates using the current pose of the ego vehicle $(x_{ego},y_{ego},\theta_{ego})$.

  2. estimate an improved (less noisy) trajectory of the leader in global coordinates.

  3. backtransform the optimized state estimates from global to ego coordinates using the corresponding ego vehicle pose.

It is possible to stay in the ego frame but this seems to need more effort, since it would be harder to tell if the leader or the ego vehicle moved. However, I was told the common approach is the solution in global coordinates.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.