A system uses range and Doppler velocity measurements from a lidar system and images from a video system to estimate a six degree-of-freedom trajectory of a target. The system estimates this trajectory in two stages: a first stage in which the range and Doppler measurements from the lidar system along with various feature measurements obtained from the images from the video system are used to estimate first stage motion aspects of the target (i.e., the trajectory of the target); and a second stage in which the images from the video system and the first stage motion aspects of the target are used to estimate second stage motion aspects of the target. Once the second stage motion aspects of the target are estimated, a three-dimensional image of the target may be generated.