FMCW frequency modulated continuous wave radar range and angular measurements, Six degrees of freedom to achieve inverse kinematics algorithm, Combination formula DSmT evidence reasoning calculation function.