US Patent 7672801 Gridlock processing method

Targets are sensed by a first sensor in a first coordinate system and by a second sensor in a second coordinate system. Gridlock or congruence between the coordinate systems is achieved by sensing at least four targets with both sensors. Pseudomeasurements are generated in both local and external coordinate systems by taking the differences between a first target location and each of second, third, and fourth target locations. The pseudomeasurements are filtered in corresponding pairs, to thereby generate an estimated rotation matrix between the local and external coordinate systems. The estimated rotation matrix is applied to the local coordinate system. The target locations in the rotated local and external coordinate systems are filtered in corresponding pairs to generate an estimated translation vector. The rotation and translation are applied to the local coordinate system to bring it into congruence with the external coordinate system.