Methods and Systems for Determining Alignment Parameters of a Radar Sensor

Abstract

A computer implemented method for determining alignment parameters of a radar sensor comprises the following steps carried out by computer hardware components: determining measurement data using the radar sensor, the measurement data comprising a range-rate measurement, an azimuth measurement, and an elevation measurement; determining a velocity of the radar sensor; and determining the misalignment parameters based on the measurement data and the velocity, the misalignment parameters comprising an azimuth misalignment, an elevation misalignment, and a roll misalignment.

Claims

1. A method, comprising: determining, by computer hardware components of a system, misalignment parameters of a radar sensor of the system, determining the misalignment parameters comprising: determining, using the radar sensor, measurement data, the measurement data comprising a range-rate measurement, an azimuth measurement, and an elevation measurement; determining a velocity of the radar sensor; and determining, based on the measurement data and the velocity, misalignment parameters the misalignment parameters comprising an azimuth misalignment, an elevation misalignment, and a roll misalignment.

2. The method of claim 1, wherein the misalignment parameters further comprise a speed-scaling error.

3. The method of claim 1, further comprising: determining, by the computer hardware components, system parameters comprising at least one of a desired azimuth mounting angle, a desired elevation mounting angle, and a desired roll mounting angle, wherein determining the misalignment parameters comprises determining the misalignment parameters further based on the system parameters.

4. The method of claim 1, wherein the measurement data is related to a plurality of objects external to the radar sensor.

5. The method of claim 4, wherein each of the plurality of objects external to the radar sensor is stationary.

6. The method of claim 1, wherein the misalignment parameters are determined based executing the steps of the method in an iterative manner.

7. The method of claim 1, further comprising: executing, by the computer hardware components, an optimization function, wherein determining the misalignment parameters comprises determining the misalignment parameters further based on executing the optimization function.

8. The method of claim 7, wherein determining the velocity of the radar sensor and determining the misalignment parameters occurs simultaneously by executing the optimization function to solve an optimization function.

9. The method of claim 1, wherein determining the misalignment parameters comprises determining the misalignment parameters further based on at least one of: executing, by the computer hardware components, a non-linear least squares regression function; executing, by the computer hardware components, a non-linear total least squares regression function; or executing, by the computer hardware components, a filtering function.

10. The method of claim 1, wherein determining the measurement data comprises determining the measurement data using a plurality of radar sensors including the radar sensor.

11. The method of claim 1, further comprising: correcting, based on misalignment parameters, the measurement data to obtain corrected measurement data.

12. The method of claim 11, wherein the system comprises a vehicle comprising a radar system that includes the radar sensor, the method further comprising: outputting, to one or more additional computer components of a vehicle, the measurement data for controlling the vehicle.

13. The method of claim 12, wherein the vehicle comprises the radar system.

14. A system, comprising: one or more computer hardware components configured to determine misalignment parameters of a radar sensor by at least: determining, using the radar sensor, measurement data, the measurement data comprising a range-rate measurement, an azimuth measurement, and an elevation measurement; determining a velocity of the radar sensor; and determining, based on the measurement data and the velocity, misalignment parameters the misalignment parameters comprising an azimuth misalignment, an elevation misalignment, and a roll misalignment.

15. The system of claim 14, further comprising: a radar system of a vehicle including the radar sensor.

16. The system of claim 15, further comprising: the vehicle including the radar sensor, the one or more computer hardware components being further configured to: correct, based on misalignment parameters, the measurement data to obtain corrected measurement data; and control, based on corrected measurement data, the vehicle.

17. The system of claim 14, wherein the misalignment parameters further comprise a speed-scaling error.

18. The system of claim 14, wherein the one or more computer hardware components are further configured to: determine system parameters comprising at least one of a desired azimuth mounting angle, a desired elevation mounting angle, and a desired roll mounting angle; and determine the misalignment parameters by at least determining the misalignment parameters further based on the system parameters.

19. The system of claim 14, wherein the measurement data is related to a plurality of objects external to the radar sensor, wherein each of the plurality of objects is stationary.

20. A non-transitory computer readable medium comprising instructions that, when executed, configure computer hardware components of a system to determine misalignment parameters of a radar sensor of the system by at least: determining, using the radar sensor, measurement data, the measurement data comprising a range-rate measurement, an azimuth measurement, and an elevation measurement; determining a velocity of the radar sensor; and determining, based on the measurement data and the velocity, misalignment parameters the misalignment parameters comprising an azimuth misalignment, an elevation misalignment, and a roll misalignment.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Exemplary embodiments and functions of the present disclosure are described herein in conjunction with the following drawings, showing schematically:

[0034] FIG. 1A an illustration of a radar field-of-view when misaligned in azimuth and elevation;

[0035] FIG. 1B an illustration of a three-dimensional misalignment in azimuth, elevation, and roll;

[0036] FIG. 2 a flow diagram illustrating an overview of methods for determining alignment parameters according to various embodiments;

[0037] FIG. 3 an illustration of a radar field-of-view after introduction of roll misalignment;

[0038] FIG. 4 an illustration of a radar field-of-view and a compensated field-of-view according to various embodiments;

[0039] FIG. 5 a diagram illustrating azimuth alignment error distribution according to various embodiments;

[0040] FIG. 6 a diagram illustrating elevation alignment error distribution according to various embodiments;

[0041] FIG. 7 a diagram illustrating roll alignment error distribution according to various embodiments;

[0042] FIG. 8 a flow diagram illustrating a method for determining alignment parameters of a radar sensor according to various embodiments;

[0043] FIG. 9 an alignment parameter estimation system according to various embodiments; and

[0044] FIG. 10 a computer system with a plurality of computer hardware components configured to carry out steps of a computer implemented method for determining alignment parameters of a radar sensor according to various embodiments;

[0045] FIG. 11 an illustration of the vehicle coordinate system location;

[0046] FIG. 12 an illustration of a sensor coordinate system with relation to the vehicle coordinate system (VCS); and

[0047] FIG. 13 an illustration of detection definition in the sensor coordinate systems.

DETAILED DESCRIPTION

[0048] Radar alignment is a process of determining the angular misalignment of radar facing direction. Correction of those mounting errors may be crucial for proper operation of radar based tracking methods and most of the feature functions that operate on detections or tracked objects provided by a radar sensor. Two main classes of methods/approaches aimed at solving misalignment problem are: [0049] 1) static alignment methods, which may also be called factory alignment, due to the reason that they require specialized equipment to perform the calibration process; most commonly used static methods use corner reflectors, Doppler generators, or steel plates; [0050] 2) dynamic alignment methods, which may also be called auto-alignment since they may not require any external (additional) equipment (devices) for the process. The radar sensor is correcting its facing angles based on observation of the environment while the car is moving (i.e. driving).

[0051] FIG. 1A and FIG. 1B shows illustrations 100 and 150 of misalignment and radar field-of-view relationship.

[0052] FIG. 1A shows an illustration 100 of a radar field-of-view 110 when misaligned in azimuth (along a horizontal line 102) and elevation (along a vertical split line 104 at the front of the vehicle, which corresponds to 0 deg). The ideal (or desired or assumed) facing angle 106 is different from the real (or actual) facing angle 108, so that a misalignment 112 is present.

[0053] FIG. 1B shows an illustration 150 of a three-dimensional misalignment in azimuth (α), elevation (β), and roll (γ). Coordinates axis are illustrated in the vehicle coordinate system (VCS), i.e. XVCS 152, YVCS 154, and ZVCS 156. The ground plane 158 is illustrated. The radar sensor may be mounted above ground plane 158 at the origin of the vehicle coordinate system. The desired boresight direction for perfect alignment would coincide with XVCS 152. However, the actual boresight direction 160 is different from XVCS 152 due to the misalignment, and the difference between the actual boresight direction 160 and XVCS 152 may be described by the three-dimensional misalignment in azimuth (α), elevation (β), and roll (γ). Due to the misalignment, the desired FOV 162 (field of view) may be different from the actual FOV 164. However, the desired FOV 162 may be obtained by correction based on misalignment parameters based on the actual FOV 164.

[0054] To calculate the misalignment in case of a misalignment in azimuth and elevation (disregarding a possible misalignment in roll), the range rate equation of stationary detections may be used. It may be derived from the projection of the negative vehicle velocity vector to the vector of distance, which forms the equation below:

{dot over (R)}=−V.sub.X cos α.sub.VCS cos β.sub.VCS−V.sub.Y sin α.sub.VCS cos β.sub.VCS−V.sub.Z sin β.sub.VCS  (1)

where: [0055] {dot over (r)} is the range rate (Doppler velocity of stationary detection); [0056] V.sub.X is the longitudinal velocity of the vehicle (which may be a vector facing forward from vehicle, parallel to the ground plane; V.sub.X may be the length of that vector); [0057] V.sub.Y is the lateral velocity of the vehicle (which may be a vector facing left side of vehicle, parallel to the ground plane; V.sub.Y may be the length of that vector); [0058] V.sub.Z is the vertical velocity of the vehicle (which may be a vector facing ground, perpendicular to the ground plane; V.sub.Z may be the length of that vector); [0059] α.sub.VCS is the azimuth measurement of detection aligned to vehicle coordinate system (wherein the 0 [deg] angle may be defined as facing the same direction as the front of the vehicle on which the radar sensor is mounted); and [0060] β.sub.VCS is the elevation measurement of detection aligned to vehicle coordinate system.

[0061] VCS aligned measurement angles may be decomposed into sensor mounting angles, sensor detection measurement angles as well as sensor mounting misalignment angles:

α.sub.VCS=α.sub.SCS+α.sub.B+a.sub.M  (2)

β.sub.VCS=β.sub.SCS+β.sub.B+β.sub.M  (3)

where: [0062] α.sub.SCS is the azimuth measurement of detection in sensor coordinate system (SCS); [0063] α.sub.B is the desired azimuth mounting angle; [0064] α.sub.M is the desired azimuth misalignment; [0065] β.sub.SCS is the elevation measurement of detection in sensor coordinate system; [0066] β.sub.B is the desired elevation mounting angle; and [0067] β.sub.M is the elevation misalignment.

[0068] Further expanding the equation with speed correction factor Vc (which may also be referred to as (vehicle) speed-scaling-error or speed compensation factor) leads to:

[00001]$\begin{array}{cc}{V}_X=\frac{V_{Xm}}{1+V_c}{V}_Y=\frac{V_{Ym}}{1+V_c}{V}_Z=\frac{V_{Zm}}{1+V_c}& \left(4\right)\end{array}$

where: [0069] V.sub.Xm is the measured longitudinal velocity of vehicle; [0070] V.sub.Ym is the measured lateral velocity of vehicle; [0071] V.sub.Zm is the measured vertical velocity of the vehicle; and [0072] V.sub.c is the speed-scaling-error.

[0073] An equation may be created which may be used to find unknown parameters (B), based on measurements (X), known constants (C) and equation result (Y) as follows:

f(C{α.sub.B,β.sub.B},X{α.sub.SCS,β.sub.SCS,V.sub.Xm,V.sub.Ym,V.sub.Zm},B{α.sub.M,β.sub.M,V.sub.c})=Y{{dot over (r)}}  (7)

[0074] Various methods, such as iterative non-linear least squares regression or error-in-variables methods, may be used to determine the unknown parameters. The parameters found by these methods may then further be refined by filtering in time methods such as Kalman filter.

[0075] FIG. 2 shows a flow diagram 200 illustrating an overview of methods for determining alignment parameters according to various embodiments (either not taking into account a misalignment in roll angle, or taking into account a misalignment in roll angle, like will be described in more detail below). At 202, data acquisition may be carried out, which may include stationary detections, vehicle velocities, and radar information. At 204, regression may be carried out, for example including estimation of azimuth misalignment, estimation of elevation misalignment, estimation of roll misalignment (as will be described in more detail below), and estimation of speed-scaling error. At 206, filtering (for example using a Kalman filter) may be provided.

[0076] Significant roll misalignment angle of the sensor (desired sensor roll is usually zero) can greatly impact the azimuth and elevation measurements, because it reduces radar field of view as shown below.

[0077] Roll angle misalignment may influence the azimuth-elevation misalignment calculation itself.

[0078] FIG. 3 shows an illustration 300 of a radar field-of-view after introduction of roll misalignment, similar to what is shown in FIG. 1A (wherein in FIG. 1A no roll misalignment is illustrated). Illustration 302 in the left portion of FIG. 3 shows an illustration from the ground perspective, and illustration 304 in the right portion of FIG. 3 shows an illustration from the sensor perspective.

[0079] As can be seen from FIG. 3, if the roll misalignment would not be taken into account, the azimuth misalignment may be taken as 0 (zero) or close to 0, and the elevation misalignment may be estimated to be greater than the correct value of the elevation misalignment, due to the fact that the ideal facing angle is exactly below the reported facing angle as seen by the radar sensor. This may result in finding wrong misalignment parameters, for example zero azimuth misalignment and too high elevation misalignment value.

[0080] The impact of the roll angle misalignment may be almost unobservable for small angles (for example less than 1 [deg]), and may be growing exponentially with the roll, introducing highest error at the corners of field-of-view. At a 30 [deg] radar roll (i.e. a misalignment of the radar sensor in roll angle of 30 deg), as is illustrated in FIG. 3 and FIG. 4, the error is reducing the field of view of radar by half (the sensor may observe ground in near vicinity of front of the vehicle, and the sky above on the opposite side of FOV; however, both the ground and the sky regions are irrelevant from the active safety methods perspective). At the same time, a significant part of important information about objects located approximately 0.5 m above the ground is missed. Furthermore, an undetected roll angle misalignment may make the azimuth and elevation misalignment angles unreliable. For example, an object at 60 deg azimuth and 5 deg elevation under influence of 5 deg sensor roll misalignment will be reported at azimuth 60.20 deg and −0.2484 deg elevation.

[0081] The radar roll angle misalignment may be determined by extending the range rate equation, which makes estimation of the roll angle possible. The equation may further improve estimation of yaw and pitch angle misalignments. After the estimation of 3 angle misalignments (yaw, pitch, roll) the measured detections may be corrected to represent detections in vehicle coordinate system or in extreme misalignment the estimated value may trigger an alert which will stop the execution of further radar functions.

[0082] The range rate equation (1) may be used together with equations (2) and (3), which may be extended with the roll angle influence, what leads to following equations:

α.sub.VCS=α.sub.SCS′+α.sub.B+α.sub.M  (8)

β.sub.VCS=β.sub.SCS′+β.sub.B+β.sub.M  (9)

α.sub.SCS′=α.sub.SCS cos(γ.sub.M+γ.sub.B)−β.sub.SCS sin(γ.sub.M+γ.sub.B)  (10)

β.sub.SCS′=α.sub.SCS sin(γ.sub.M+γ.sub.B)+β.sub.SCS cos(γ.sub.M+γ.sub.B)  (11)

where: [0083] α′.sub.SCS is the azimuth measurement corrected by roll misalignment in sensor coordinate system; [0084] β′.sub.SCS is the elevation measurement corrected by roll misalignment in sensor coordinate system; [0085] γ.sub.M is the roll misalignment; and [0086] γ.sub.B is the roll ideal mounting position (which may usually be 0 deg or 180 deg).

[0087] Therefore, α′ SCS and β′ SCS may be visualized as detections with the same distance from radar center of view (0, 0), but rotated (aligned) with regard to a horizon line (assuming desired roll mounting angle of 0 [deg]), forming a ‘compensated field of view’ as shown on FIG. 4.

[0088] FIG. 4 shows an illustration 400 of a radar field-of-view 110 and a compensated field-of-view 402 according to various embodiments. Various portions of FIG. 4 are similar or identical to portions of FIG. 1A, so that the same reference signs may be used and duplicate description may be omitted.

[0089] After substitution of eqns. (8) and (9) (α′ and β′ SCS) to eqns. (2) (3) for αSCS and βSCS, respectively, the following equations may be obtained:

α.sub.VCS=α.sub.SCS cos(γ.sub.M+γ.sub.B)−β.sub.SCS sin(γ.sub.M+γ.sub.B)+α.sub.B+α.sub.M  (12)

β.sub.VCS=α.sub.SCS sin(γ.sub.M+γ.sub.B)+β.sub.SCS cos(γ.sub.M+γ.sub.B)+β.sub.B+β.sub.M  (13)

[0090] This change modifies the general model in equation (7) to:

ƒ(C{α.sub.B,β.sub.B,γ.sub.B},C{α.sub.SCS,β.sub.SCS,V.sub.Xm,V.sub.Ym,V.sub.Zm},B{α.sub.M,β.sub.M,γ.sub.M,V.sub.c})=Y{{dot over (r)}}  (14)

[0091] The general form of this model can be derived from range rate equation (1)

[00002]$\begin{array}{cc}\stackrel{.}{r}=-\frac{V_{\mathrm{Xm}}}{1+V_c}\cos \left({\alpha }_{\mathrm{SCS}}\cos \right)\left({\gamma }_M+{\gamma }_B\right)-{\beta }_{\mathrm{SCS}}\sin \left({\gamma }_M+{\gamma }_B\right)+{\alpha }_B+{\alpha }_M).Math.\cos \left({\alpha }_{\mathrm{SCS}}\sin \left({\gamma }_M+{\gamma }_B\right)+{\beta }_{\mathrm{SCS}}\cos \left({\gamma }_M+{\gamma }_B\right)+{\beta }_B+{\beta }_M\right)-\frac{V_{\mathrm{Ym}}}{1+\mathrm{Vc}}\sin \left({\alpha }_{\mathrm{SCS}}\cos \left({\gamma }_M+{\gamma }_B\right)-{\beta }_{\mathrm{SCS}}\sin \left({\gamma }_M+{\gamma }_B\right)+{\alpha }_B+{\alpha }_M\right).Math.\cos \left({\alpha }_{\mathrm{SCS}}\sin \left({\gamma }_M+{\gamma }_B\right)+{\beta }_{\mathrm{SCS}}\cos \left({\gamma }_M+{\gamma }_B\right)+{\beta }_B+{\beta }_M\right)-\frac{V_{\mathrm{Zm}}}{1+V_c}\sin \left({\alpha }_{\mathrm{SCS}}\sin \right)\left({\gamma }_M+{\gamma }_B\right)+{\beta }_{\mathrm{SCS}}\cos \left({\gamma }_M+{\gamma }_B\right)+{\beta }_B+{\beta }_M)& \left(15\right)\end{array}$

[0092] Eq. (13) can be simplified with the assumption of the vehicle moving only in forward direction (VYm and VZm equal to 0) and elevation and roll desired mounting angles θ [deg], to get the following form:

[00003]$\begin{array}{cc}\stackrel{.}{r}=-\frac{V_{Xm}}{1+V_c}\cos \left({\alpha }_{SCS}cos{\gamma }_M-{\beta }_{SCS}\sin {\gamma }_M+{\alpha }_B+{\alpha }_M\right).Math.\cos \left({\alpha }_{SCS}\sin {\gamma }_M+{\beta }_{SCS}\cos {\gamma }_M+{\beta }_M\right)& \left(16\right)\end{array}$

[0093] Equation (16) may be used to compose (create) an overdetermined system of equations which can be solved by various methods to find parameters of interest i.e. misalignment angles and speed compensation factor. In an example, equation (14) may be used as a model for non-linear least square regression. For example, the model may be iterative, which means that the method will iterate over the same dataset containing data from one time instance to converge to the solution of non-linear equation in multiple steps defined by linearization of the model around its operating point:

(J.sup.T.Math.J)ΔB=J.sup.T.Math.ΔY  (17)

[0094] The matrix equation (17) can be transformed to:

ΔB=(J.sup.T.Math.J).sup.−1.Math.J.sup.T.Math.ΔY  (18)

where: [0095] ΔB is the update of parameters value in a single iteration (single step in a solution space); [0096] J are the partial derivatives of model function over the parameters (which may be referred to as Jacobian matrix); and [0097] ΔY is the difference between the measured output of the system and model output:

[00004]$\begin{array}{cc}\Delta Y=\left[\begin{array}{c}{\stackrel{.}{r}}_1-f_1\\ \mathrm{.Math.}\\ {\stackrel{.}{r}}_n-f_n\end{array}\right]& \left(19\right)\end{array}$

[0098] The Jacobian matrix may take the following form:

[00005]$\begin{array}{cc}J=\left[\begin{array}{cccc}\frac{\partial f_1}{\partial V_c}& \frac{\partial f_1}{\partial {\alpha }_M}& \frac{\partial f_1}{\partial {\beta }_M}& \frac{\partial f_1}{\partial {\gamma }_M}\\ \mathrm{.Math.}& \mathrm{.Math.}& \mathrm{.Math.}& \mathrm{.Math.}\\ \frac{\partial f_n}{\partial V_c}& \frac{\partial f_n}{\partial {\alpha }_M}& \frac{\partial f_n}{\partial {\beta }_M}& \frac{\partial f_n}{\partial {\gamma }_M}\end{array}\right]& \left(20\right)\end{array}$

where:

[00006]$\begin{array}{cc}\frac{\partial f_i}{\partial V_c}=\frac{V_{\mathrm{Xm}}}{{\left(1+V_c\right)}^2}\cos \left({\alpha }_{\mathrm{SCS}}^i\cos {\gamma }_M-{\beta }_{\mathrm{SCS}}^i\sin {\gamma }_M+{\alpha }_B+{\alpha }_M\right).Math.\cos \left({\alpha }_{\mathrm{SCS}}^i\sin {\gamma }_M+{\beta }_{\mathrm{SCS}}^i\cos {\gamma }_M+{\beta }_M\right)& \left(21\right)\\ \frac{\partial f_i}{\partial {\alpha }_M}=\frac{V_{\mathrm{Xm}}}{1+V_c}\cos \left({\alpha }_{\mathrm{SCS}}^i\cos {\gamma }_M-{\beta }_{\mathrm{SCS}}^i\sin {\gamma }_M+{\alpha }_B+{\alpha }_M\right).Math.\cos \left({\alpha }_{\mathrm{SCS}}^i\sin {\gamma }_M+{\beta }_{\mathrm{SCS}}^i\cos {\gamma }_M+{\beta }_M\right)& \left(22\right)\\ \frac{\partial f_i}{\partial {\alpha }_M}=\frac{V_{\mathrm{Xm}}}{1+V_c}\cos \left({\alpha }_{\mathrm{SCS}}^i\cos {\gamma }_M-{\beta }_{\mathrm{SCS}}^i\sin {\gamma }_M+{\alpha }_B+{\alpha }_M\right).Math.\sin \left({\alpha }_{\mathrm{SCS}}^i\sin {\gamma }_M+{\beta }_{\mathrm{SCS}}^i\cos {\gamma }_M+{\beta }_M\right)& \left(23\right)\\ \frac{\partial f_i}{\partial {\gamma }_M}=\frac{V_{\mathrm{Xm}}}{1+V_c}\cos \left({\alpha }_{\mathrm{SCS}}^i\cos {\gamma }_M-{\beta }_{\mathrm{SCS}}^i\sin {\gamma }_M+{\alpha }_B+{\alpha }_M\right).Math.\sin \left({\alpha }_{\mathrm{SCS}}^i\sin {\gamma }_M+{\beta }_{\mathrm{SCS}}^i\cos {\gamma }_M+{\beta }_M\right).Math.\left({\alpha }_{\mathrm{SCS}}^i\cos {\gamma }_M-{\beta }_{\mathrm{SCS}}^i\sin {\gamma }_M\right)-\frac{V_{\mathrm{Xm}}}{1+V_c}\sin \left({\alpha }_{\mathrm{SCS}}^i\cos {\gamma }_M-{\beta }_{\mathrm{SCS}}^i\sin {\gamma }_M+{\alpha }_B+{\alpha }_M\right).Math.\cos ({\alpha }_{\mathrm{SCS}}^i\sin {\gamma }_M+{\beta }_{\mathrm{SCS}}^i\cos {\gamma }_M+{\beta }_M).Math.\left({\alpha }_{\mathrm{SCS}}^i\sin {\gamma }_M+{\beta }_{\mathrm{SCS}}^i\cos {\gamma }_M\right)& \left(24\right)\end{array}$

[0099] The non-linear regression model described by equations (18) to (24) may be solved after assuming initial values of parameters:

[00007]$\begin{array}{cc}{B}_0=\{\begin{array}{c}[\begin{array}{c}0\\ 0\\ 0\\ 0\end{array}]\mathrm{for}m=0\\ [\begin{array}{c}{V}_{\mathrm{cm}-1}\\ {\alpha }_{\mathrm{Mm}-1}\\ {\beta }_{\mathrm{Mm}-1}\\ {\gamma }_{\mathrm{Mm}-1}\end{array}]\mathrm{for}m>1\end{array}& \left(25\right)\end{array}$

[0100] where m is the number of successful alignment cycles (radar cycles). In other words, a misalignment of 0 in the azimuth, elevation and roll angles and in the speed-correction factor may be assumed as the starting point for the first iteration, and the results of the previous iteration may be used as starting point for the next iteration.

[0101] Therefore, equation (26) may update model parameter values with values at i-th iteration:

B.sub.i=B.sub.i-1+ΔB.sub.i  (26)

[0102] Bi may converge to real speed-scaling-error and misalignment angles at infinite number of iterations (or as iteration number approaches infinity):

[00008]$\begin{array}{cc}\underset{i.fwdarw.\infty }{\lim }{B}_i=\left[\begin{array}{c}{V}_c\\ {\alpha }_M\\ {\beta }_M\\ {\gamma }_M\end{array}\right]& \left(27\right)\end{array}$

[0103] The method according to various embodiments provides alignment parameters also for the roll axis, which accurately describes the physical model.

[0104] FIG. 5, FIG. 6, and FIG. 7 illustrate the performance of methods according to various embodiments over simulated data for random (nonzero) misalignments of all mounting angles i.e. azimuth, elevation, and roll. The following simulation parameters were used: [0105] number of test cases (samples): 500 000 (same inputs for each method); [0106] misalignments (azimuth, elevation, and roll): pseudorandom values drawn from uniform distribution on interval [−5:5] deg; [0107] azimuth boresight pseudorandom value drawn from uniform distribution on interval [−180; 180] deg; [0108] vehicle moving in a straight line (X.sub.VCS direction); [0109] minimum vehicle speed: 11 m/s; [0110] number of detections: 64; [0111] azimuth Gaussian zero mean noise: Standard deviation 0.3 deg; [0112] elevation Gaussian zero mean noise: Standard deviation 1 deg; [0113] range rate Gaussian zero mean noise: Standard deviation 0.03 m/s; [0114] vehicle velocity Gaussian zero mean noise in 3-axis: Standard deviation 0.2 m/s.

[0115] The result of only a single iteration (first iteration) of each of compared method is shown in each of FIG. 5, FIG. 6, and FIG. 7. NL-LS in the method name may stand for non-linear least squares regression used to find the solution, and NL-TLS may stand for non-linear total least squares regression. A 2-axial alignment with no roll estimation is shown for comparison purposes.

[0116] FIG. 5 shows a diagram 500 illustrating azimuth alignment error distribution according to various embodiments. The deviation angle is illustrated over a horizontal axis 502. The number of samples is illustrated over a vertical axis 504. The results for a 2 axial alignment are illustrated by curve 506. The results for a 3 axial alignment NL-LS method according to various embodiments are illustrated by curve 508. The results for a 3 axial alignment NL-TLS method according to various embodiments are illustrated by curve 510.

[0117] FIG. 6 shows a diagram 600 illustrating elevation alignment error distribution according to various embodiments. The deviation angle is illustrated over a horizontal axis 602. The number of samples is illustrated over a vertical axis 604. The results for a 2 axial alignment are illustrated by curve 606. The results for a 3 axial alignment NL-LS method according to various embodiments are illustrated by curve 608. The results for a 3 axial alignment NL-TLS method according to various embodiments are illustrated by curve 610.

[0118] FIG. 7 shows a diagram 700 illustrating roll alignment error distribution according to various embodiments. The deviation angle is illustrated over a horizontal axis 702. The number of samples is illustrated over a vertical axis 704. The results for a 3 axial alignment NL-LS method according to various embodiments are illustrated by curve 706. The results for a 3 axial alignment NL-TLS method according to various embodiments are illustrated by curve 708.

[0119] As shown in FIG. 5 and FIG. 6, even one-time-instance-based (non-iterative) results are much better in case of azimuth and elevation misalignment estimation with 3-axial alignment compared to a 2-axial alignment.

[0120] FIG. 7 shows the error distribution of roll angle alignment, in this case 2 axial alignment is not available as it does not return the roll angle estimation.

[0121] FIG. 8 shows a flow diagram 800 illustrating a method for determining alignment parameters of a radar sensor according to various embodiments. At 802, measurement data may be determined using the radar sensor, wherein the measurement data may include a range-rate measurement, an azimuth measurement, and an elevation measurement. At 804, a velocity of the radar sensor may be determined. At 806, the misalignment parameters may be determined based on the measurement data and the velocity, wherein the misalignment parameters may include an azimuth misalignment, an elevation misalignment, and a roll misalignment.

[0122] According to various embodiments, the misalignment parameters may further include a speed-scaling error.

[0123] According to various embodiments, system parameters may be determined, wherein the system parameters may include at least one of a desired azimuth mounting angle, a desired elevation mounting angle, and a desired roll mounting angle. The misalignment parameters may be determined further based on the system parameters.

[0124] According to various embodiments, the measurement data may be related to a plurality of objects external to the radar sensor.

[0125] According to various embodiments, the plurality of objects may be stationary.

[0126] According to various embodiments, the misalignment parameters may be determined based on an iterative method.

[0127] According to various embodiments, the misalignment parameters may be determined based on an optimization method.

[0128] According to various embodiments, the misalignment parameters may be determined based on a non-linear least squares regression method.

[0129] According to various embodiments, the misalignment parameters may be determined based on a non-linear total least squares regression method.

[0130] According to various embodiments, the misalignment parameters may be determined based on filtering.

[0131] According to various embodiments, the measurement data may be determined using a plurality of radar sensors.

[0132] According to various embodiments, the velocity of the radar sensor and the misalignment parameters may be determined simultaneously by solving an optimization problem.

[0133] Each of the steps 802, 804, 806 and the further steps described above may be performed by computer hardware components.

[0134] FIG. 9 shows an alignment parameter estimation system 900 according to various embodiments. The alignment parameter estimation system 900 may include a measurement data determination circuit 902, a velocity determination circuit 904, and a misalignment parameters determination circuit 906.

[0135] The measurement data determination circuit 902 may be configured to determine measurement data using a radar sensor, the measurement data comprising a range-rate measurement, an azimuth measurement, and an elevation measurement. The radar sensor may be a part of the measurement data determination circuit 902 or may be provided external to the measurement data determination circuit 902.

[0136] The velocity determination circuit 904 may be configured to determine a velocity of the radar sensor. The velocity sensor may be a part of the velocity determination circuit 904 or may be provided external to the velocity determination circuit 904.

[0137] The misalignment parameters determination circuit 906 may be configured to determine misalignment parameters of the radar sensor based on the measurement data and the velocity, the misalignment parameters comprising an azimuth misalignment, an elevation misalignment, and a roll misalignment.

[0138] The measurement data determination circuit 902, the velocity determination circuit 904, and the misalignment parameters determination circuit 906 may be coupled with each other, e.g. via an electrical connection 908, such as e.g. a cable or a computer bus or via any other suitable electrical connection to exchange electrical signals.

[0139] A “circuit” may be understood as any kind of a logic implementing entity, which may be special purpose circuitry or a processor executing a program stored in a memory, firmware, or any combination thereof.

[0140] FIG. 10 shows a computer system 1000 with a plurality of computer hardware components configured to carry out steps of a computer implemented method for determining alignment parameters of a radar sensor according to various embodiments. The computer system 1000 may include a processor 1002, a memory 1004, and a non-transitory data storage 1006. A radar sensor 1008 and/or a velocity sensor 1010 may be provided as part of the computer system 1000 (like illustrated in FIG. 10) or may be provided external to the computer system 1000.

[0141] The processor 1002 may carry out instructions provided in the memory 1004. The non-transitory data storage 1006 may store a computer program, including the instructions that may be transferred to the memory 1004 and then executed by the processor 1002. The radar sensor 1008 may be used for determining measurement data as described above. The velocity sensor 1010 may be used to determine the velocity as described above.

[0142] The processor 1002, the memory 1004, and the non-transitory data storage 1006 may be coupled with each other, e.g. via an electrical connection 1012, such as e.g. a cable or a computer bus or via any other suitable electrical connection to exchange electrical signals. The radar sensor 1008 and/or the velocity sensor 1010 may be coupled to the computer system 1000, for example via an external interface, or may be provided as parts of the computer system (in other words: internal to the computer system, for example coupled via the electrical connection 1012).

[0143] The terms “coupling” or “connection” are intended to include a direct “coupling” (for example via a physical link) or direct “connection” as well as an indirect “coupling” or indirect “connection” (for example via a logical link), respectively.

[0144] It will be understood that what has been described for one of the methods above may analogously hold true for the alignment parameter estimation system 900 and/or for the computer system 1000.

[0145] In the following, further embodiments will be described. These embodiments may provide simultaneous estimation of host vehicle velocity and radar misalignment based on multiple Doppler radars detections.

[0146] As used herein, various coordinate systems may be used.

[0147] An ISO—earth fixed coordinate system may be based on an axis system which remains fixed in the inertial reference frame. The origin of this coordinate system may be fixed on the ground plane. The position and orientation may be chosen in an arbitrary manner (for example based on the desired application). The naming in ISO standard for the system may be (xE; yE; zE).

[0148] A further coordinate system may be the vehicle coordinate system.

[0149] FIG. 11 shows an illustration 1100 of the vehicle coordinate system location. The vehicle coordinate system may include a right hand sided axis trio: (XV; YV; ZV). The values in coordinate system may be denoted as (xV; yV; zV). The vehicle coordinate system may be in the center of the rear axle for a vehicle in a steady state. The coordinate system may remain fixed to the vehicle sprung weight meaning that during motion or on inclinations it may not be aligned with the rear axle. The location of the VCS on the vehicle body at rest with respect to vehicle dimensions may be as presented in FIG. 11, where in a side view in the top portion of FIG. 11, the host vehicle 1102 at rest is shown on a ground plane 1104, with rear overhang 1106 and a distance 1108 between rear axle and front bumper. At top view of shown in the bottom portion of FIG. 11, with the sprung weight centerline 1110 indicated. The axis (XV; YV; ZV) are also shown in FIG. 11.

[0150] A further coordinate system may be the sensor coordinate system.

[0151] Sensors like LiDARs and radars report detections in the polar coordinate system. Each detection may be described by the trio of: range; azimuth; elevation.

[0152] FIG. 12 shows an illustration 1200 of a sensor coordinate system with relation to the vehicle coordinate system (VCS). The origin of the sensor coordinate system may be defined in respect to the VCS by translation 1202 and rotation 1204.

[0153] FIG. 13 shows an illustration 1300 of detection definition in the sensor coordinate systems. The sensor origin 1302, the X axis 1304, Y axis 1306, and Z axis 1308 are shown. Detection azimuth 1310 may be defined as an angle between the X axis 1304 and the projection 1318 of the line connecting the origin 1302 and the detection 1316 on the (X; Y) plane. Detection elevation 1312 may be defined as an angle between the (X; Z) plane and the line 1318 connecting the origin and the detection 1316. Detection range 1314 may be defined as the cartesian distance between the sensor origin 1302 and the detection 1316.

[0154] Every radar scan may produce a set of (radar) detections, where each of the detections may have following properties: [0155] range (which may be the distance from sensor origin to detection, as illustrated in FIG. 13); [0156] range rate (which may be the radial velocity of detection; in other words: the temporal derivative of the range); [0157] azimuth (for example as shown in FIG. 13); and [0158] elevation (for example as shown in FIG. 13).

[0159] Target planar motion may be described by:

V.sup.t=[ω.sup.tx.sup.ty.sup.t].sup.T

[0160] The range rate equation for a single raw detection from target may be given as:

{dot over (r)}.sup.i+v.sub.x.sup.s cos θ.sup.i+v.sub.y.sup.s sin θ.sup.i=v.sub.x.sup.t cos θ.sup.i+v.sub.y.sup.t,sin θ.sup.i

where: [0161] {dot over (r)}.sup.i—i-th detection range rate, [0162] θ.sup.i—i-th detection azimuth.

[0163] To simplify the notation, the notion of a compensated range rate may be introduced and defined as:

{dot over (r)}.sub.cmp.sup.i={dot over (r)}.sup.i+v.sub.x.sup.s cos θ.sup.i+v.sub.y.sup.h sin θ.sup.i

[0164] Then range rate equation may be reduced to:

{dot over (r)}.sub.cmp.sup.i=v.sub.x.sup.t cos θ.sup.i+v.sub.x.sup.t sin θ.sup.i

[0165] In vector form, it may be described as follows:

[00009]${\stackrel{.}{r}}_{cmp}^i=\left[\cos {\theta }^i\sin {\theta }^i\right]\left[\begin{array}{c}{v}_x^t\\ v_y^t\end{array}\right]$

Coefficients v.sub.x.sup.t and v.sub.y.sup.t, may be called velocity profile.

[0166] The velocity profile may be successfully estimated if at least 2 detections from one target object are available. This estimation can be done by application of least squares method or method presented in US 2019/0369228. Both methods may provide an estimation of velocity profile and its covariance matrix:

[00010]${\sigma }_{VP}=\left[\begin{array}{cc}& \\ {\sigma }_{\widehat{v_x^t}\widehat{v_y^t}}& \end{array}\right]$

[0167] The Gauss-Newton optimization method may be used for minimization of a function in quadratic form:

Q(μ)=ƒ(μ).sup.Tƒ(μ)

[0168] The Gauss-Newton optimization method may include the following steps:

1. The point μ.sup.0, function ƒ(μ) and its Jacobian matrix

[00011]$J\left(\mu \right)=\frac{\partial f\left(\mu \right)}{\partial \mu }$

may be given initially. Set i=0.
2. Set γ.sup.i=1.

3. Compute:

[0169]
μ.sup.i+1=μ.sup.i+γ.sup.i(J(μ.sup.i).sup.TJ(μ.sup.i)).sup.−1J(μ.sup.i).sup.Tƒ(μ.sup.i)

4. If Q(μ.sup.i+1)>Q(μ.sup.i), set γ.sup.i=½ γ.sup.i and repeat from 3.
5. Terminate if stop criteria reached.
6. Otherwise set i=i+1 and repeat from 2.

[0170] According to various embodiments, devices and methods may be provided for simultaneous estimation of vehicle velocity and radars misalignment angles. Such an approach may not be possible for a single radar, because only by having multiple radars, the measurement from one sensor may be corrected based on other sensors measurements.

[0171] In the following, the method for the 2D (two-dimensional) case will be described.

[0172] Range rate equation in the 2D case may have the following form:

[00012]$-\stackrel{.}{r}=v_{s,x}^{scs}\cos \theta +v_{s,y}^{scs}\sin \theta =\left[\cos \theta \sin \theta \right]\left[\begin{array}{c}{v}_{s,x}^{scs}\\ v_{s,y}^{scs}\end{array}\right]$

where:

[0173] {dot over (r)} may be the range rate of detection,

[0174] θ may be the azimuth of detection,

[0175] [v.sub.s,x.sup.scs v.sub.s,y.sup.scs].sup.T may be the sensor velocity reported in Sensor Coordinate System (SCS).

[0176] If α is sensor yaw angle, then the detection range rate may be a function of sensor velocity in the Vehicle Coordinate System (VCS) and may have the following form:

[00013]$-\stackrel{.}{r}=\left[\cos \theta \sin \theta \right][\begin{array}{cc}\cos \alpha & \sin \alpha \\ -\sin \alpha & \cos \alpha \end{array}][\begin{array}{c}{v}_{s,x}^{vcs}\\ v_{s,y}^{vcs}\end{array}]=\hspace{1em}=\left[\cos \left(\theta +\alpha \right)\sin \left(\theta +\alpha \right)\right][\begin{array}{c}{v}_{s,x}^{vcs}\\ v_{s,y}^{vcs}\end{array}]$

where [0177] [v.sub.s,x.sup.scs v.sub.s,y.sup.scs].sup.T may be the sensor velocity reported in Vehicle Coordinate System (VCS), and [0178] the sensor yaw angle α may be the sum of the calibration angle α.sub.0 and the misalignment angle α.sub.m.

α=α.sub.0+α.sub.m

[0179] The sensor velocity may be also presented as function of host velocity in VCS:

[00014]$\left\{\begin{array}{c}{v}_{s,x}^{vcs}=v_{h,x}^{vcs}-\omega \Delta y\\ v_{s,x}^{vcs}=v_{h,y}^{vcs}+\omega \Delta x\end{array}\right\}$

where [0180] [v.sub.h,x.sup.vcs, v.sub.h,y.sup.vcs].sup.T may be the host velocity reported in Vehicle Coordinate System (VCS), [0181] ω may be the host yaw rate, [0182] Δx, Δy may be the position of radar in VCS.

[0183] Based on those equation range rate equation may be written as:

[00015]$-\stackrel{.}{r}=\left[\cos \left({\alpha }_0+{\alpha }_m+\theta \right)\sin \left({\alpha }_0+{\alpha }_m+\theta \right)\right]\left[\begin{array}{c}{v}_{h,x}^{vcs}-\omega \Delta y\\ v_{h,y}^{vcs}+\omega \Delta x\end{array}\right]$

[0184] Considering all static detections from all radars may provide the following system of equations:

[00016]$\left[\begin{array}{c}-{\stackrel{.}{r}}_1^1\\ -{\stackrel{.}{r}}_2^1\\ \mathrm{.Math.}\\ -{\stackrel{.}{r}}_{N_1}^1\\ -{\stackrel{.}{r}}_1^2\\ \mathrm{.Math.}\\ -{\stackrel{.}{r}}_{N_M}^M\end{array}\right]=\hspace{1em}[\begin{array}{c}\cos \left({\alpha }_0^1+{\alpha }_m^1+{\theta }_1^1\right)\left[v_{h,x}^{vcs}-\omega \Delta y\right]+\sin \left({\alpha }_0^1+{\alpha }_m^1+{\theta }_1^1\right)\left[v_{h,y}^{vcs}+\omega \Delta x\right]\\ \cos \left({\alpha }_0^1+{\alpha }_m^1+{\theta }_2^1\right)\left[v_{h,x}^{vcs}-\omega \Delta y\right]+\sin \left({\alpha }_0^1+{\alpha }_m^1+{\theta }_2^1\right)\left[v_{h,y}^{vcs}+\omega \Delta x\right]\\ \mathrm{.Math.}\\ \cos \left({\alpha }_0^1+{\alpha }_m^1+{\theta }_{N_1}^1\right)\left[v_{h,x}^{vcs}-\omega \Delta y\right]+\sin \left({\alpha }_0^1+{\alpha }_m^1+{\theta }_{N_1}^1\right)\left[v_{h,y}^{vcs}+\omega \Delta x\right]\\ \cos \left({\alpha }_0^2+{\alpha }_m^2+{\theta }_1^2\right)\left[v_{h,x}^{vcs}-\omega \Delta y\right]+\sin \left({\alpha }_0^2+{\alpha }_m^2+{\theta }_1^2\right)\left[v_{h,y}^{vcs}+\omega \Delta x\right]\\ \mathrm{.Math.}\\ \cos \left({\alpha }_0^M+{\alpha }_m^M+{\theta }_{N_M}^M\right)\left[v_{h,x}^{vcs}-\omega \Delta y\right]+\sin \left({\alpha }_0^M+{\alpha }_m^M+{\theta }_{N_M}^M\right)\left[v_{h,y}^{vcs}+\omega \Delta x\right]\end{array}]$

[0185] In short, this may be written as:

−{dot over (r)}=h(ρ)

where

[00017]$\overline{\rho }=\left[\begin{array}{c}{v}_{h,x}^{vcs}\\ v_{h,x}^{vcs}\\ \omega \\ {\alpha }_m^1\\ \mathrm{.Math.}\\ {\alpha }_m^M\end{array}\right]$

[0186] Based on this, the vector p of host velocity and radar alignments may be found by optimization of the following function:

ƒ(ρ)=h(ρ)+{dot over (r)}

[0187] This optimization may be done by the Gauss-Newton method as described above, where the Jacobian matrix has the following form:

[00018]$J\left(\overline{\rho }\right)=\left[\begin{array}{ccc}\frac{\partial h_1^1\left(\overline{\rho }\right)}{\partial v_{h,x}^{vcs}}& \mathrm{.Math.}& \frac{\partial h_1^1\left(\overline{\rho }\right)}{\partial {\alpha }_m^M}\\ \mathrm{.Math.}& \ddots & \mathrm{.Math.}\\ \frac{\partial h_{N_M}^M\left(\overline{\rho }\right)}{\partial v_{h,x}^{vcs}}& \mathrm{.Math.}& \frac{\partial h_{N_M}^M\left(\overline{\rho }\right)}{\partial {\alpha }_m^M}\end{array}\right]$

where:

[00019]$\frac{\partial h_k^l\left(\stackrel{\_}{\rho }\right)}{\partial v_{h,x}^{vcs}}=\cos \left({\alpha }_0^k+{\alpha }_m^k+{\theta }_k^l\right),\frac{\partial h_k^l\left(\stackrel{\_}{\rho }\right)}{\partial v_{h,y}^{vcs}}=\sin \left({\alpha }_0^k+{\alpha }_m^k+{\theta }_k^l\right),\frac{\partial h_k^l\left(\stackrel{\_}{\rho }\right)}{\partial \omega }=\sin \left({\alpha }_0^k+{\alpha }_m^k+{\theta }_k^l\right)\Delta x-\cos \left({\alpha }_0^k+{\alpha }_m^k+{\theta }_k^l\right)\Delta y,\frac{\partial h_k^l\left(\stackrel{\_}{\rho }\right)}{\partial {\alpha }_m^k}=-\sin \left({\alpha }_0^k+{\alpha }_m^k+{\theta }_k^l\right)\left[v_{h,x}^{vcs}-\omega \Delta y\right]+\cos \left({\alpha }_0^k+{\alpha }_m^k+{\theta }_k^l\right)\left[v_{h,y}^{vcs}+\omega \Delta x\right],\mathrm{otherwise}\frac{\partial h_k^l\left(\stackrel{\_}{\rho }\right)}{\partial {\alpha }_m^i}=0.$

[0188] Based on this Jacobian matrix, the Hessian matrix may be approximated in the following form:

[00020]${J\left(\overline{\rho }\right)}^{T}J\left(\overline{\rho }\right)=\left[\begin{array}{ccc}{H}_{11}& \mathrm{.Math.}& H_{1(M+3)}\\ \mathrm{.Math.}& \ddots & \mathrm{.Math.}\\ *& \mathrm{.Math.}& H_{(M+3)(M+3)}\end{array}\right]$

where:

[00021]$H_{ij}=\underset{k=1}{\overset{M}{.Math.}}\left\{\underset{l=1}{\overset{N_k}{.Math.}}\left[\frac{\partial h_k^l}{\partial {\rho }_i}\frac{\partial h_k^l}{\partial {\rho }_j}\right]\right\}$

Moreover, for the optimization method, the matrix

J(ρ).sup.Tƒ({dot over (r)}+h(ρ))

may be needed, and it may be calculated as:

[00022]${J\left(\overline{\rho }\right)}^{T}f\left(\stackrel{.}{\overline{r}}+h\left(\overline{\rho }\right)\right)=\left[\begin{array}{c}\underset{k=1}{\overset{M}{.Math.}}\left\{\underset{l=1}{\overset{N_k}{.Math.}}\left[\frac{\partial h_k^l}{\partial {\rho }_1}\left({\stackrel{.}{r}}_k^l+h_k^l\right)\right]\right\}\\ \mathrm{.Math.}\\ \underset{k=1}{\overset{M}{.Math.}}\left\{\underset{l=1}{\overset{N_k}{.Math.}}\left[\frac{\partial h_k^l}{\partial {\rho }_{(3+{.Math.}_{j=1}^M{N}_j)}}\left({\stackrel{.}{r}}_k^l+h_k^l\right)\right]\right\}\end{array}\right]$

[0189] In the following, the 3D case will be described.

[0190] In the 3D case, the range rate equation may have the following form:

[00023]$-\stackrel{.}{r}=\cos \mathrm{\theta sin\phi }{v}_{s,x}^{scs}+\sin \mathrm{\theta cos\phi }{v}_{s,y}^{scs}-\sin \phi v_{s,z}^{scs}=\left[\cos \mathrm{\theta sin\phi }\sin \mathrm{\theta cos\phi }-\sin \phi \right]\left[\begin{array}{c}{v}_{s,x}^{scs}\\ v_{s,y}^{scs}\\ v_{s,z}^{scs}\end{array}\right]$

where: [0191] [v.sub.s,x.sup.scs v.sub.s,y.sup.scs v.sub.s,z.sup.scs].sup.T may be the sensor velocity in Sensor Coordinate System (SCS), [0192] θ may be the azimuth of detection, [0193] φ may be the elevation of detection.

[0194] In this case, radar orientation may be assumed to be in 3D, so it may have yaw, pitch, and roll angle:

[00024]$\overline{\alpha }=\left[\begin{array}{c}{\alpha }_r\\ {\alpha }_p\\ {\alpha }_y\end{array}\right]$

[0195] Also, in this case real orientation may be the sum of calibrated orientation and misalignment:

[00025]$\overline{\alpha }={\overline{\alpha }}_0+{\overline{\alpha }}_m=\left[\begin{array}{c}{\alpha }_{0,r}\\ {\alpha }_{0,p}\\ {\alpha }_{0,y}\end{array}\right]+\left[\begin{array}{c}{\alpha }_{m,r}\\ {\alpha }_{m,p}\\ {\alpha }_{m,y}\end{array}\right]$

[0196] Taking this orientation into account, the range rate may be presented by sensor velocity in VCS by the following equation:

[00026]$-\stackrel{.}{r}=\left[\cos \mathrm{\theta sin\phi }\sin \mathrm{\theta cos\phi }-\sin \phi \right]{R\left(\overline{\alpha }\right)}^T\left[\begin{array}{c}{v}_{s,x}^{vcs}\\ v_{s,y}^{vcs}\\ v_{s,z}^{vcs}\end{array}\right]$

where [0197] [v.sub.s,x.sup.vcs v.sub.s,y.sup.vcs v.sub.s,z.sup.vcs].sup.T may be the velocity of sensor in Vehicle Coordinate System (VCS), [0198] R (α) may be a rotation 3D matrix obtained by rotation around axes in following order: OZ, OY, OX.

[0199] To simplify calculations, the following assumptions may be reasonable:

α.sub.m,⋅≈0,

α.sub.0,r=0,

v.sub.s,z.sup.vcs≈0.

[0200] It will be understood that the dot in the first assumption may stand for r and/or p, and/or y.

[0201] Then the rotation matrix may have the following form:

[00027]$R\left(\overline{\alpha }\right)=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$

[0202] where:

r.sub.11=cos α.sub.0,y−α.sub.m,y sin α.sub.0,y,

r.sub.21=(sin α.sub.0,y+α.sub.m,y cos α.sub.0,y)(cos α.sub.p,0−α.sub.m,p sin α.sub.0,p),

r.sub.31=sin α.sub.0,p+α.sub.m,p cos α.sub.0,p,

r.sub.12=(sin α.sub.0,y+α.sub.m,y cos α.sub.0,y)+α.sub.m,r(cos α.sub.0,y−α.sub.m,y sin α.sub.0,y)(sin α.sub.0,p+α.sub.m,p cos α.sub.0,p),

r.sub.22=(cos α.sub.0,y−α.sub.m,y sin α.sub.0,y)−α.sub.m,r(sin α.sub.0,y−α.sub.m,y cos α.sub.0,y)(sin α.sub.0,p+α.sub.mp cos α.sub.0,p),

r.sub.32=−α.sub.m,r(cos α.sub.0,p−α.sub.m,p sin α.sub.0,p),

r.sub.13=(sin α.sub.0,y+α.sub.m,y cos α.sub.0,y)−α.sub.m,r(cos α.sub.0,y−α.sub.m,y sin α.sub.0,y)(sin α.sub.0,p+α.sub.m,p cos α.sub.0,p),

r.sub.23=−α.sub.m,r(cos α.sub.0,y−α.sub.m,y sin α.sub.0,y)+(sin α.sub.0,y+α.sub.m,y cos α.sub.0,y)(sin α.sub.0,p+α.sub.0,p cos α.sub.0,p),

r.sub.33=cos α.sub.0,p−α.sub.m,p sin α.sub.0,p.

[0203] Summing up all of these equations, the range rate equation may be written as:

−{dot over (r)}=v.sub.s,x.sup.vcs(cos θ cos φr.sub.11+sin θ cos φr.sub.21−sin φr.sub.31)+v.sub.h,y.sup.vcs(cos θ cos φr.sub.12+sin θ cos φr.sub.22−sin φr.sub.32)

[0204] Considering all detections from all radars, an optimization problem may be formulated for finding the host velocity and radars misalignments angles (which may be similar to the 2D case as described above):

−{dot over (r)}=h(γ)

where:

[00028]$\overline{\gamma }=\left[\begin{array}{c}{v}_{hx}^{vcs}\\ v_{hy}^{vcs}\\ \omega \\ {\alpha }_{m,y}^1\\ {\alpha }_{m,p}^1\\ {\alpha }_{m,r}^1\\ {\alpha }_{m,y}^2\\ \mathrm{.Math.}\\ {\alpha }_{m,p}^M\\ {\alpha }_{m,r}^M\end{array}\right]$

[0205] This may be solved by a nonlinear least squares method with usage of a Gauss-Newton method, as described above, for which the Jacobian may be provided in the following form:

[00029]$J\left(\overline{\gamma }\right)=\left[\begin{array}{ccc}\frac{\partial h_1^1\left(\overline{\gamma }\right)}{\partial v_{h,x}^{vcs}}& \mathrm{.Math.}& \frac{\partial h_1^1\left(\overline{\gamma }\right)}{\partial {\alpha }_m^M}\\ \mathrm{.Math.}& \ddots & \mathrm{.Math.}\\ \frac{\partial h_{N_M}^M\left(\overline{\gamma }\right)}{\partial v_{h,x}^{vcs}}& \mathrm{.Math.}& \frac{\partial h_{N_M}^M\left(\overline{\gamma }\right)}{\partial {\alpha }_m^M}\end{array}\right]$

where the parameters of the Jacobian are defined as:

[00030]$\frac{\partial h\left(v\right)}{\partial v_{\mathrm{sx}}^{vcs}}=\cos {\theta }_i\cos {\phi }_i\left(c_{{\alpha }_y,0}-{\alpha }_{y,m}{s}_{{\alpha }_y,0}\right)+\sin {\theta }_i\cos {\phi }_i\left(-\left(s_{{\alpha }_y,0}+{\alpha }_{y,m}{c}_{{\alpha }_y,0}\right)\left(c_{{\alpha }_p,0}-{\alpha }_{p,m}{s}_{{\alpha }_p,0}\right)\right)-\sin {\phi }_i\left(s_{{\alpha }_p,0}+{\alpha }_{p,m}{c}_{{\alpha }_p,0}\right),\frac{\partial h\left(v\right)}{\partial v_{\mathrm{sy}}^{vcs}}=\cos {\theta }_i\cos {\phi }_i\left(\left(s_{{\alpha }_y,0}+{\alpha }_{y,m}{c}_{{\alpha }_y,0}\right)+{\alpha }_{r,m}\left(c_{{\alpha }_y,0}-{\alpha }_{y,m}{s}_{{\alpha }_y,0}\right)\left(s_{{\alpha }_p,0}+{\alpha }_{p,m}{c}_{{\alpha }_p,0}\right)\right)+\hspace{1em}\sin {\theta }_i\cos {\phi }_i(\left(c_{{\alpha }_y,0}-{\alpha }_{y,m}{s}_{{\alpha }_y,0}\right)-{\alpha }_{r,m}\left(s_{{\alpha }_y,0}+\hspace{1em}{\alpha }_{y,m}{c}_{{\alpha }_y,0})\left(s_{{\alpha }_p,0}+{\alpha }_{p,m}{c}_{{\alpha }_p,0}\right)\right)+\sin {\phi }_i\left({\alpha }_{r,m}\left(c_{{\alpha }_p,0}+{\alpha }_{p,m}{s}_{{\alpha }_p,0}\right)\right),\frac{\partial h\left(v\right)}{\partial \omega }=\Delta x\frac{\partial h\left(v\right)}{\partial v_{\mathrm{sy}}^{vcs}}-\Delta y\frac{\partial h\left(v\right)}{\partial v_{\mathrm{sx}}^{vcs}},\frac{\partial h\left(v\right)}{\partial {\alpha }_{y,m}^i}=-\left(v_{\mathrm{sx}}^{\mathrm{vcs}}-\Delta y\omega \right)\left[c_1{s}_{{\alpha }_{y,0}^i}+c_2{c}_{{\alpha }_{y,0}^i}\left(c_{{\alpha }_{y,0}^i}-{\alpha }_{p,m}^i{s}_{{\alpha }_{p,0}^i}\right)\right]+\left(v_{\mathrm{sy}}^{\mathrm{vcs}}+\Delta x\omega \right)\left[c_1\left(c_{{\alpha }_{y,0}^i}-s_{{\alpha }_{y,0}^i}\left(s_{{\alpha }_{p,0}^i}+{\alpha }_{p,m}{c}_{{\alpha }_{p,0}^i}\right)\right)-c_2\left(s_{{\alpha }_{y,0}^i}+{\alpha }_{r,m}^i{c}_{{\alpha }_{y,0}^i}\left(s_{{\alpha }_{y,0}^i}+{\alpha }_{p,m}^i{c}_{{\alpha }_{p,0}^i}\right)\right)\right],\frac{\partial h\left(v\right)}{\partial {\alpha }_{p,m}^i}=-\left(v_{\mathrm{sx}}^{\mathrm{vcs}}-\Delta y\omega \right)\left[c_2{\alpha }_{p,0}^i\left(s_{{\alpha }_{y,0}^i}-{\alpha }_{y,m}^i{c}_{{\alpha }_{y,0}^i}\right)+c_3{c}_{{\alpha }_{p,0}^i}\right]+\left(v_{\mathrm{sx}}^{\mathrm{vcs}}+\Delta x\omega \right)\left[c_1{c}_{{\alpha }_{p,0}^i}{\alpha }_{r,m}^i\left(c_{{\alpha }_{y,0}^i}-{\alpha }_{y,m}^i{s}_{{\alpha }_{y,0}^i}\right)+c_2{\alpha }_{r,m}^i{c}_{{\alpha }_{p,0}^i}\left(s_{{\alpha }_{y,0}^i}-{\alpha }_{y,m}^i{c}_{{\alpha }_{y,0}^i}\right)+c_3{\alpha }_{r,m}^i{c}_{{\alpha }_{p,0}^i}\right],\frac{\partial h\left(v\right)}{\partial {\alpha }_{r,m}^i}=-\left(v_{\mathrm{sx}}^{\mathrm{vcs}}-\Delta x\omega \right)\left[c_1\left(c_{{\alpha }_{y,0}^i}-{\alpha }_{y,m}^i{s}_{{\alpha }_{y,0}^i}\right)\left(s_{{\alpha }_{p,0}^i}+{\alpha }_{p,o}^i{c}_{{\alpha }_{p,0}^i}\right)-c_2\left(s_{{\alpha }_{y,0}^i}+{\alpha }_{y,m}^i{c}_{{\alpha }_{y,0}^i}\right)\left(s_{{\alpha }_{p,0}^i}+{\alpha }_{p,m}^i{c}_{{\alpha }_{p,0}^i}\right)-c_3\left(c_{{\alpha }_{p,0}^i}-{\alpha }_{p,m}^i{s}_{{\alpha }_{y,0}^i}\right)\right].$