METHOD FOR DETERMINING A MOVEMENT STATE OF A RIGID BODY

Abstract

A method for determining a movement state of a rigid body relative to an environment using a multiplicity of measurement data sets relating to objects in the environment around the body. Each measurement data set includes a measurement time, a Doppler velocity, and an azimuth angle in relation to a respective sensor reference system. The method includes determining a movement state of the body relative to the environment as a velocity vector and an angular velocity vector in a body reference system. At least one set of conditions that includes a plurality of measurement data sets is created. A function dependent on Doppler velocity deviations between estimated Doppler velocities and the Doppler velocities of the measurement data sets included in the set of conditions is minimized in a regression analysis for the set of conditions. The estimated Doppler velocities are regarded as dependent variables in the regression analysis.

Claims

1-21. (canceled)

22. A method for determining a movement state of a rigid body relative to an environment using a multiplicity of measurement data sets relating to objects in the environment around the body, wherein each of the measurement data sets includes a measurement time, a Doppler velocity, and an azimuth angle in relation to a sensor reference system of a sensor, the method comprising the following steps: determining the movement state of the body relative to the environment as a velocity vector and an angular velocity vector in a body reference system, wherein each sensor reference system can be translated into the body reference system by a non-singular transformation, the determining of the movement state including: creating at least one set of conditions that includes a plurality of the measurement data sets, and minimizing in a regression analysis for the at least one set of conditions a functional that is dependent on Doppler velocity deviations between estimated Doppler velocities and the Doppler velocities of the measurement data sets included in the at least one set of conditions, wherein the estimated Doppler velocities are represented as dependent variables in the regression analysis, wherein one or more components of the velocity vector and/or of the angular velocity vector are determined by the regression analysis.

23. The method as recited in claim 22, wherein at least one of the measurement data sets includes an elevation angle, and wherein at least some of the one or more measurement data sets are determined from measurement results that include only a measurement time, the Doppler velocity, and the azimuth angle, wherein the elevation angle is set to be the same as a predetermined value which is exactly zero.

24. The method as recited in claim 22, wherein the measurement data sets are captured using one or more sensors that are attached to the body and capture the environment around the body.

25. The method as recited in claim 22, wherein the measurement data sets include at least two measurement data sets from a sensor having different measurement times.

26. The method as recited in claim 22, wherein the measurement times of the measurement data sets from at least two different sensors are different from one another.

27. The method as recited in claim 22, wherein the movement state for one determination time is determined at least at one calculation time, and wherein the at least one set of conditions is created from the measurement data sets whose measurement times are within a determination period.

28. The method as recited in claim 27, wherein, in the functional, the Doppler velocity deviations are multiplied by a positive time weight that is a function of a time difference between the measurement time of a measurement data set and the determination time for which the movement state is determined, wherein the time weight decreases as a magnitude of the time difference increases.

29. The method as recited in claim 27, wherein the movement state for a plurality of successive determination times is determined, wherein each determination period of the plurality of successive determination times has a lower time limit and an upper time limit, wherein the lower time limit is identical to a previous determination time of the plurality of successive determination times and the upper time limit is identical to a subsequent determination time of the plurality of successive determination times or to the calculation time.

30. The method as recited in claim 27, wherein a first determination of the movement state for the determination time is carried out based on a first determination period, and a second determination of the movement state for the determination time is carried out based on a second determination period, wherein the second determination period is different from the first determination period and includes at least one measurement time that is not included in the first determination period, and wherein the first determination is carried out at a first calculation time and the second determination is carried out at a second, later calculation time, wherein an upper time limit of the second determination period comes after the first calculation time.

31. The method as recited in claim 22, wherein the estimated Doppler velocities are also represented in the regression analysis as being dependent on the velocity vector and the angular velocity vector by using the transformation between the respective sensor reference system and the body reference system, and the velocity vector and the angular velocity vector are determined such that the functional is minimized.

32. The method as recited in claim 22, wherein: a plurality of sets of conditions are created which each include measurement data sets from just one sensor; a plurality of sensor velocity vectors are determined in each sensor reference system by also representing the estimated Doppler velocities as being dependent on the sensor velocity vector for each of the sets of conditions in the regression analysis, and the body sensor velocity vector is determined such that the respective functional is minimized; and in a regression, the velocity vector and the angular velocity vector are determined such that a regression functional, which is dependent on sensor deviations between the sensor velocity vectors and estimated sensor velocity vectors, is minimized, wherein the sensor velocity vectors are also represented as being dependent on the velocity vector and the angular velocity vector by using the transformation between the body reference system and the respective sensor reference system.

33. The method as recited in claim 32, wherein the movement state for one determination time is determined at least at one calculation time, and wherein the at least one set of conditions is created from the measurement data sets whose measurement times are within a determination period, wherein each of the sets of conditions only includes measurement data sets that have the same measurement time, and wherein in the regression functional, the sensor deviations are each multiplied by a weight that is a function of the time difference between the measurement time of the set of conditions and the determination time for which the movement state is determined.

34. The method as recited in claim 32, wherein the sets of conditions include at least one component constraint on at least one component of the velocity vector and/or of the angular velocity vector in the form of at least one component default value; wherein the functional or the regression functional is also dependent on a component deviation between the at least one component default value and at least one estimated component default value.

35. The method as recited in claim 34, wherein the movement state for one determination time is determined at least at one calculation time, and wherein the at least one set of conditions is created from the measurement data sets whose measurement times are within a determination period, wherein at least one angular velocity component, which is determined by an angular rate sensor, is used in the body reference system as a component default value, and wherein, in the functional or the regression functional, the at least one component deviation is multiplied by a weight that is a function of a time difference between a measurement time of the angular velocity component and the determination time for which the movement state is determined.

36. The method as recited in claim 22, wherein: the measurement data sets each include at least one additional parameter selected from: a distance from a captured object, and/or a variance in the distance, and/or a variance in the azimuth angle, and/or a variance in the elevation angle, and/or a variance in the Doppler velocity, and/or a signal strength of a received signal, and/or a cross section, and/or a radar cross section or LiDAR cross section, in each case based on the captured object of the measurement data set, and/or a type of sensor, and/or an arrangement of the sensor on the body, and wherein the Doppler velocity deviations are multiplied by an additional weight that is a function of the additional parameter, and/or a measurement data set is rejected when the at least one additional parameter is outside at least one predetermined range.

37. The method as recited in claim 22, wherein: i) the regression analysis is carried out using an error-in-variables regression method, and/or ii) the azimuth angle is optimized, and/or the regression analysis is carried out using an iteratively reweighted least squares method.

38. A method for determining a relative position and/or a relative orientation of a rigid body, comprising the following steps: determining a plurality of movement states of the body for a plurality of successive determination times, each of the movement states being determined relative to an environment using a multiplicity of measurement data sets relating to objects in the environment around the body, wherein each of the measurement data sets includes a measurement time, a Doppler velocity, and an azimuth angle in relation to a sensor reference system of a sensor, each movement state being determined by: determining the movement state of the body relative to the environment as a velocity vector and an angular velocity vector in a body reference system, wherein each sensor reference system can be translated into the body reference system by a non-singular transformation, the determining of the movement state including: creating at least one set of conditions that includes a plurality of the measurement data sets, and minimizing in a regression analysis for the at least one set of conditions a functional that is dependent on Doppler velocity deviations between estimated Doppler velocities and the Doppler velocities of the measurement data sets included in the at least one set of conditions, wherein the estimated Doppler velocities are represented as dependent variables in the regression analysis, wherein one or more components of the velocity vector and/or of the angular velocity vector are determined by the regression analysis; integrating the movement states over time between a starting determination time of the plurality of determination times and an end determination time of the plurality of determination times to obtain the relative position and/or the relative orientation as results of the integration.

39. An arithmetic logic unit configured to obtain a multiplicity of measurement data sets and configured to determine a movement state of a rigid body relative to an environment using the multiplicity of measurement data sets what relate to objects in the environment around the body, wherein each of the measurement data sets includes a measurement time, a Doppler velocity, and an azimuth angle in relation to a sensor reference system of a sensor, the arithmetic logic unit configured to: determine the movement state of the body relative to the environment as a velocity vector and an angular velocity vector in a body reference system, wherein each sensor reference system can be translated into the body reference system by a non-singular transformation, the determining of the movement state including: creating at least one set of conditions that includes a plurality of the measurement data sets, and minimizing in a regression analysis for the at least one set of conditions a functional that is dependent on Doppler velocity deviations between estimated Doppler velocities and the Doppler velocities of the measurement data sets included in the at least one set of conditions, wherein the estimated Doppler velocities are represented as dependent variables in the regression analysis, wherein one or more components of the velocity vector and/or of the angular velocity vector are determined by the regression analysis.

40. A vehicle and/or a robot, comprising: one or more sensors that are attached to a body of the vehicle and/or robot, and that capture an environment around the body, the one or more sensors being configured to take measurements of objects in the environment and to send measurement data sets captured to the arithmetic logic unit; and an arithmetic logic unit configured to obtain a multiplicity of the measurement data sets and configured to determine a movement state of a rigid body relative to an environment using the multiplicity of measurement data sets what relate to objects in the environment around the body, wherein each of the measurement data sets includes a measurement time, a Doppler velocity, and an azimuth angle in relation to a sensor reference system of a sensor of the one or more sensors, the arithmetic logic unit configured to: determine the movement state of the body relative to the environment as a velocity vector and an angular velocity vector in a body reference system, wherein each sensor reference system can be translated into the body reference system by a non-singular transformation, the determining of the movement state including: creating at least one set of conditions that includes a plurality of the measurement data sets, and minimizing in a regression analysis for the at least one set of conditions a functional that is dependent on Doppler velocity deviations between estimated Doppler velocities and the Doppler velocities of the measurement data sets included in the at least one set of conditions, wherein the estimated Doppler velocities are represented as dependent variables in the regression analysis, wherein one or more components of the velocity vector and/or of the angular velocity vector are determined by the regression analysis.

41. The vehicle and/or robot as recited in claim 40, wherein the one or more sensors are radar sensors and/or LiDAR sensors

42. A non-transitory machine-readable storage medium on which is stored a computer program for determining a movement state of a rigid body relative to an environment using a multiplicity of measurement data sets relating to objects in the environment around the body, wherein each of the measurement data sets includes a measurement time, a Doppler velocity, and an azimuth angle in relation to a sensor reference system of a sensor, the computer program, when executed by an arithmetic logic unit, causing the arithmetic logic unit to perform the following steps: determining the movement state of the body relative to the environment as a velocity vector and an angular velocity vector in a body reference system, wherein each sensor reference system can be translated into the body reference system by a non-singular transformation, the determining of the movement state including: creating at least one set of conditions that includes a plurality of the measurement data sets, and minimizing in a regression analysis for the at least one set of conditions a functional that is dependent on Doppler velocity deviations between estimated Doppler velocities and the Doppler velocities of the measurement data sets included in the at least one set of conditions, wherein the estimated Doppler velocities are represented as dependent variables in the regression analysis, wherein one or more components of the velocity vector and/or of the angular velocity vector are determined by the regression analysis.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0037] FIG. 1 is a plan view of a vehicle to which sensors are attached.

[0038] FIG. 2 shows a reference system of a sensor that captures an object.

[0039] FIG. 3 shows a diagram having a plurality of determination and measurement times, according to an example embodiment of the present invention.

[0040] FIG. 4 shows a flowchart according to a preferred specific embodiment of the present invention.

DETAILED DESCRIPTION OF EXAMPLE EMBODIMENTS

[0041] FIG. 1 is a plan view of a motor vehicle 2 (as an example of a rigid body) to which sensors S1, S2, S3, S4 are attached (e.g., radar sensors and/or LiDAR sensors). Other vehicles such as aircraft and watercraft are also possible as rigid bodies. Robots are equally possible. In particular, the vehicle can be automated (robotic vehicle). Generally speaking, the present invention can also be used with rigid bodies that are not self-driving. The motor vehicle (more generally the body) defines a body reference system B having three mutually orthogonal axes, i.e., an x axis, a y axis, and a z axis, which in the plan view extends out of the plane of the drawing perpendicularly thereto. For a motor vehicle or a vehicle, the origin U.sub.B of the body reference system B is preferably placed at the center of rotation of the vehicle. In a motor vehicle having Ackermann steering, this is the middle of the rear axle, as shown.

[0042] The rigid body, i.e., the motor vehicle 2, moves in an environment in which objects 4 such as trees, buildings, and other motor vehicles are found. In this case, both static objects, i.e., objects that are not moving in the environment, and moving objects can be present. The static objects are located at fixed positions in the environment or in an environment reference system.

[0043] The body (motor vehicle 2) moves in the environment. This movement, i.e., the movement state of the body in the environment reference system, can be represented in the body reference system B using a linear velocity vector (V.sub.x, V.sub.y, V.sub.z) and an angular velocity vector (?.sub.x, ?.sub.y, ?.sub.z); the components of the velocity vector represent the velocities of the body in the environment reference system but along the respective axes x, y, z of the body reference system, and the components of the angular velocity vector represent the angular velocities of the body in the environment reference system but about the respective axes x, y, z of the body reference system. Only V.sub.x, V.sub.y and ?.sub.z are indicated in the view in FIG. 1.

[0044] The sensors S1, S2, S3, S4 are attached in certain positions on the body, in this case at the four corners of the motor vehicle 2. A different number of sensors is also possible, the number of sensors preferably being in the range from 1 to 10, more preferably in the range from 3 to 6. By contrast with what is shown, sensors can be attached not only to corners but also to any other sites on the rigid body.

[0045] Each of the sensors defines a sensor reference system. In FIG. 1, a sensor reference system S is indicated to represent one of the sensors S1, said system having mutually orthogonal axes, i.e., an x axis x.sub.s, a y axis y.sub.s, and a z axis z.sub.s. The z axis is not indicated in the plan view and can, but need not, extend perpendicularly to the plane of the drawing or in parallel with the z axis of the body reference system.

[0046] The sensor reference system of each of the sensors is linked to the body reference system by way of a linear transformation, i.e., can be mapped therein by the transformation. Vectors/coordinates in each of the sensor reference systems can thus be mapped into corresponding vectors/coordinates in the body reference system by the relevant transformation. The transformation for each of the sensor reference systems is in each case specified by combining a translation t (characterized by a three-dimensional translation vector between the origin U.sub.B of the body reference system and an origin U.sub.s of the sensor reference system) and a rotation R (characterized by a Euler angle, for example) (again indicated as representing one of the sensors). The transformation is thus a transformation between two three-dimensional spaces. It should be non-singular.

[0047] In addition, the vehicle in FIG. 1 preferably comprises an arithmetic logic unit 12 (for example a controller) that is configured to receive measurement data sets from the sensors and to determine a movement state on that basis, the arithmetic logic unit being configured to carry out a regression analysis, in particular as described in more detail below.

[0048] FIG. 2 shows a reference system S of a sensor (a radar sensor and/or a LiDAR sensor), i.e., a sensor reference system, that captures an object 4. The sensor reference system S has an x axis x.sub.s, a y axis y.sub.s, and a z axis z.sub.s, which are mutually orthogonal. By way of example, a schematically shown sensor surface 6 of the sensor is arranged in the y-z plane in this case and captures the half-space having coordinates x.sub.s>0 as the field of vision. It goes without saying that the sensors can also have a different field of vision (other than a half-space). The sensor or sensors used for the present invention are configured to determine, by making use of the Doppler effect, a velocity of a captured object, referred to as the Doppler velocity, toward or away from the sensor; in other words, Doppler sensors are used. Preferably, the sensors are radar sensors or Doppler radar sensors and/or LiDAR sensors.

[0049] The position of the object 4 is specified by an object position vector P that extends from the origin U.sub.s of the sensor reference system to the object 4. Said object position vector can be indicated by spherical coordinates (?, ?, r) in the sensor reference system. The radius r=|P|.sub.2 is the length of the object position vector P, i.e., the distance between the origin U.sub.s and the object 4. The azimuth angle ? is the angle between the x axis x.sub.s and the projection 8 of the object position vector P into the x-y plane. The elevation angle ? is the angle between the x-y plane and the object position vector P. Accordingly:

P=(x.sub.S, y.sub.S, z.sub.S)=(r cos ? cos ?, r sin ? cos ?, r sin ?)

[0050] The object 4 has a velocity, characterized by an object velocity vector V.sub.o, relative to the sensor or in the sensor reference system. The velocity of the sensor relative to the object is accordingly specified by V.sub.s=?V.sub.o. The Doppler velocity d is specified by the length of the projection of the object velocity vector V.sub.o onto the object position vector P. This length is equal to the scalar product d=p.sup.0. V.sub.o between the object velocity vector V.sub.o and a unit vector p.sup.0 in the direction of the object position vector P, where p.sup.0=P/r. Overall, therefore, the Doppler velocity d can be expressed by the velocity vector V.sub.s=(V.sub.Sx, V.sub.Sy, V.sub.Sz) of the sensor relative to the object:

[00001]$\begin{array}{c}-d=\left(\cos ?\cos ?,\sin ?\cos ?,\sin ?\right)\left(\begin{array}{c}{V}_{\mathrm{Sx}}\\ V_{\mathrm{Sy}}\\ V_{\mathrm{Sz}}\end{array}\right)\\ =V_{\mathrm{Sx}}\cos ?\cos ?+V_{\mathrm{Sy}}\sin ?\cos ?+V_{\mathrm{Sz}}\sin ?\end{array}$

[0051] The sensor captures a multiplicity of measurement data sets (e.g., 50 to 150 measurement data sets) at a single measurement time, and so with each measurement by the sensor a multiplicity of Doppler velocities and corresponding azimuth and elevation angles are ascertained. The static objects define the environment relative to which the velocity of the sensor, and more broadly of the rigid body, can be determined by way of the measured Doppler velocities.

[0052] To be able to clearly represent the relevant equations despite the high number of measurement data sets, a suitable notation is introduced hereinafter.

[0053] Vectors, matrixes, and scalars: vectors and matrixes are shown in bold to make them easier to distinguish from scalars.

[0054] Points in the three-dimensional (3D) space: a point p to be expressed in Cartesian coordinates in relation to a coordinate system or reference system A is written as .sub.Ap.

[0055] Vectors between two points in the 3D space: a translation vector between two points B and C in relation to a reference system A is written as .sub.At.sub.B,C.

[0056] Velocities of the reference system in the 3D space: if a reference system C moves relative to a reference system B at a particular linear or angular velocity, the relevant velocity in relation to the relative reference system A for linear velocities is written as:

.sub.Av.sub.B,C

and for angular velocities it is written as:

.sub.A?.sub.B,C

[0057] Rotations and transformations of the rigid body: a transformation between two reference systems A and B is written as:

T.SUB.A,B

[0058] A transformation of this kind transforms points expressed in reference system B into reference system A:

T.sub.A,B.Math..sub.Bp=.sub.Ap

[0059] Since transformations include a rotation and a translation, this can also be written as:

R.sub.A,B.Math..sub.Bp+.sub.At.sub.A,B=.sub.Ap

where R.sub.A,B is the corresponding turning or rotation matrix. T.sub.A,B can be regarded as a position and orientation of one reference system relative to the other (also referred to as the pose).

[0060] The reference system of the rigid body, i.e., the body reference system, is denoted by B. The reference system of a sensor, i.e., a sensor reference system, is denoted by S, S1, S2, . . . Si, . . . SI, the sensors themselves also in part being denoted by S, S1, S2, . . . Si, . . . SI for the sake of simplicity. The reference system of the environment is denoted by W.

[0061] Expressed in Cartesian coordinates or polar coordinates and using the present notation, the position of an object in the reference system Si of a sensor is as follows:

.sub.sip.sub.j=(x,y,z).sup.T=r(cos ? cos ?, sin ? cos ?, sin ?).sup.T

where j denotes a measurement data set (of the multiplicity thereof) of an object (e.g., results of a radar measurement or LiDAR measurement). If a sensor Si moves at a velocity .sub.Siv.sub.W,Si relative to the environment W in which the object is a static object, equation (1) applies as described above:

[00002]$d_j=-{\left({}_{\mathrm{Si}}{p}_j^0\right)}^T{}_{\mathrm{Si}}{v}_{W,\mathrm{Si}}\mathrm{where}{p}^0=\frac{p}{{.Math.p.Math.}_2}={\left(\cos ?\cos ?,\sin ?\cos ?,\sin ?\right)}^T$

are the normalized point coordinates of the object, i.e., the unit vector in the direction of the object (|?|.sub.2 denotes the usual 2-norm, i.e., the length of the vector).

[0062] From a plurality of measurement data sets of a plurality of Doppler velocities d.sub.j, where j is selected from 1, 2, . . . J (J is the number of measurement data sets used; in this case a plurality of measurement data sets of a single object and/or measurement data sets of different objects can be included at one measurement time), the following equation system (2) is produced:

[00003]$\left(\begin{array}{ccc}\cos {?}_1\cos {?}_1& \sin {?}_1\cos {?}_1& \sin {?}_1\\ .Math.& .Math.& .Math.\\ \cos {?}_J\cos {?}_J& \sin {?}_J\cos {?}_J& \sin {?}_1\end{array}\right)\left(\begin{array}{c}{}_{\mathrm{Si}}{\mathrm{vx}}_{W,\mathrm{Si}}\\ {}_{\mathrm{Si}}{\mathrm{vy}}_{W,\mathrm{Si}}\\ {}_{\mathrm{Si}}{\mathrm{vz}}_{W,\mathrm{Si}}\end{array}\right)=\left(\begin{array}{c}-d_1\\ .Math.\\ -d_J\end{array}\right)$

[0063] If measurement data sets do not include an elevation angle ?, the corresponding elevation angles ?.sub.j are assumed or set to be equal to a predetermined angle in this equation system. Preferably, these elevation angles are set to be exactly zero (?.sub.j=0 for measurement data sets j for which there is no elevation angle). This leads to the equation system being simplified since the cosine and sine of said elevation angles are accordingly 1 and 0, respectively. In simpler notation, equation system (2) can be written as:

[00004]$\left(\begin{array}{c}{\left(p_1^0\right)}^T\\ .Math.\\ {\left(p_j^0\right)}^T\end{array}\right){}_{\mathrm{Si}}{v}_{W,\mathrm{Si}}=\left(\begin{array}{c}-d_1\\ .Math.\\ -d_J\end{array}\right)$$A.Math.{}_{\mathrm{Si}}{v}_{W,\mathrm{Si}}=D$

where A and D are used to denote the respective vectors in the equation system; these are specified by measured values from the sensors.

[0064] A results from the azimuth angle ? and the elevation angle ? of a measurement data set, and D results from the corresponding measured Doppler velocities d.sub.j.

[0065] Therefore, when a multiplicity of measurement data sets from a sensor Si are available, the velocity .sub.Siv.sub.W,Si of the sensor can be estimated or determined by solving said equations, the equations generally creating an overdetermined equation system. The measurement data sets can be regarded as conditions for the sensor velocity that are to be satisfied in terms of the above equation system. The multitude of measurement data sets or at least a part thereof thus represents a set of conditions.

[0066] The above equation system (1) can be solved by way of a regression, the sensor velocity .sub.Siv.sub.W,Si being determined or varied such as to minimize an error measure, i.e., a regression functional F, which is dependent on a difference (Doppler velocity deviation (d.sub.j?{circumflex over (d)}.sub.j)) between the measured Doppler velocities d.sub.j and the estimated Doppler velocities {circumflex over (d)}.sub.j, which are specified by the left-hand side of the equation system for each sensor velocity. By way of example, a mean square error could be minimized:

[00005]$F=F\left[\left(d_1-{\stackrel{?}{d}}_1\right),.Math.,\left(d_j-{\stackrel{?}{d}}_j\right),.Math.,\left(d_J-{\stackrel{?}{d}}_J\right)\right]?\underset{j}{.Math.}{\left(d_j-{\stackrel{?}{d}}_j\right)}^2$

[0067] It goes without saying that functional dependencies other than squares are also possible.

[0068] In addition, weights g.sub.j which are dependent on additional parameters that are additionally captured by the sensor during a measurement (e.g., radar measurement or LiDAR measurement) can be provided in this case. The weights g.sub.j can also be referred to as parameter weights. In the functional, the difference between the measured Doppler velocities d.sub.j and the estimated Doppler velocities {circumflex over (d)}.sub.j is then multiplied by the corresponding weight g.sub.j. This results in the following:

[00006]$F=F\left[g_1\left(d_1-{\stackrel{?}{d}}_1\right),.Math.,g_j\left(d_j-{\stackrel{?}{d}}_j\right),.Math.,g_J\left(d_J-{\stackrel{?}{d}}_J\right)\right]?\underset{j}{.Math.}{g_j^2\left(d_j-{\stackrel{?}{d}}_j\right)}^2$

[0069] By way of example, a measurement data set in which the azimuth and/or elevation angle have a high degree of uncertainty or variance can be given a lower weight relative to measurement data sets in which there is a low degree of variance.

[0070] Depending on additional parameters, individual measurement data sets can also remain disregarded, i.e., are not incorporated in the above equation system (2), for example if a distance from an object is above a predetermined threshold. Setting the corresponding (parameter) weight g.sub.j to exactly zero has a similar effect.

[0071] Generally speaking, the measurement data sets contain errors; in particular, not all the captured objects will generally be static objects, and so there will be outliers in the measurement data sets that distort the determination of the sensor velocity. To minimize this effect, a robust regression method is preferably used. In particular, an iteratively reweighted least squares (IRLS) method can be used. Such IRLS methods are conventional to the person skilled in the art.

[0072] To determine the linear velocity vector .sub.Bv.sub.W,B and the angular velocity vector .sub.B?.sub.W,B of the rigid body, the velocity of the sensor is correlated with the (linear and angular) velocity vectors of the body, the relative rotation R.sub.B,Si and translation .sub.Bt.sub.B,Si of the sensors fixedly attached to the body being used; equation (3):

.sub.Siv.sub.W,Si=R.sub.Si,B(.sub.Bv.sub.W,B+.sub.B?.sub.W,B?.sub.Bt.sub.B,Si)

[0073] In this case, x denotes the usual cross product of two three-dimensional vectors.

[0074] Using the notation:

[00007]${.Math.a.Math.}_x=\left(\begin{array}{ccc}0& -a_z& a_y\\ a_z& 0& -a_x\\ -a_y& a_x& 0\end{array}\right)$

this equation can be written as follows:

[00008]${}_{\mathrm{Si}}{v}_{W,\mathrm{Si}}=\left[{\begin{array}{cc}{R}_{\mathrm{Si},B}& -R_{\mathrm{Si},B}.Math.{}_B{t}_{B,\mathrm{Si}}.Math.\end{array}}_x\right]\left[\begin{array}{c}{}_B{v}_{W,B}\\ {}_B{?}_{W,B}\end{array}\right]$

[0075] The velocity of a sensor can thus be expressed by a vector that includes the linear and angular velocity vectors of the rigid body.

[0076] For a plurality of sensors 1, 2, . . . I, the corresponding equations can be written in combined form as equation system (4):

[00009]$\left[\begin{array}{c}{}_{S1}{v}_{W,S1}\\ .Math.\\ {}_{\mathrm{SI}}{v}_{W,\mathrm{SI}}\end{array}\right]=M\left[\begin{array}{c}{}_B{v}_{W,B}\\ {}_B{?}_{W,B}\end{array}\right]$

where the individual transformations (relative rotations and translations) of the various sensors are combined in the matrix M. Since the matrix M is known, this results in an equation system from which the velocity vectors .sub.Bv.sub.W,B and .sub.B?.sub.W,B of the body can be determined, the equation system generally being overdetermined when there is an adequately high number of sensors.

[0077] One option for determining the movement state of the body, i.e., for determining the linear velocity vector .sub.Bv.sub.W,B and the angular velocity vector .sub.B?.sub.W,B, is first to determine the velocities .sub.Siv.sub.W,Si of the sensors as explained above, i.e., to solve the above equation system (2) by way of a regression method for each of the sensors, and then to solve the above equation system (4), with a regression (e.g., the least squares method) being used again. Overall, this two-stage process represents a regression analysis. This two-stage approach can be referred to as a weak-coupling approach and has the advantage that it can be carried out relatively quickly, i.e., can be carried out using relatively little computing power by comparison with the other approaches.

[0078] Another approach, the so-called strong-coupling approach, involves determining the linear velocity vector .sub.Bv.sub.W,B and the angular velocity vector .sub.B?.sub.W,B directly from the Doppler velocities, i.e., without determining the sensor velocities from the Doppler velocities beforehand, using a regression method that represents a regression analysis or at least part thereof. The strong-coupling approach generally leads to the movement state being determined more accurately.

[0079] For this purpose, the above equation (1) for the Doppler velocity and the above equation (3), which combines the sensor velocity with the velocity vectors of the rigid body, become:

[00010]$-d_j={\left({}_{\mathrm{Si}}{p}_j^0\right)}^T{R}_{\mathrm{Si},B}\left({}_B{v}_{W,B}+{}_B{?}_{W,B}?{}_B{t}_{B,\mathrm{Si}}\right)$$-d_j=\left[\begin{array}{cc}{\left({}_{\mathrm{Si}}{p}_j^0\right)}^T{R}_{\mathrm{Si},B}& -{\left({}_{\mathrm{Si}}{p}_j^0\right)}^T{R}_{\mathrm{Si},B}\end{array}{.Math.{}_B{t}_{B,\mathrm{Si}}.Math.}_x\right]\left[\begin{array}{c}{}_B{v}_{W,B}\\ {}_B{?}_{W,B}\end{array}\right]$

[0080] If there is a multiplicity of measurement data sets 1, . . . J, said equations can be combined into the following equation system (5):

[00011]$\left(\begin{array}{c}-d_1\\ .Math.\\ -d_J\end{array}\right)=\left[\begin{array}{cc}{\left({}_{\mathrm{Si}}{p}_1^0\right)}^T{R}_{\mathrm{Si},B}& -{\left({}_{\mathrm{Si}}{p}_1^0\right)}^T{R}_{\mathrm{Si},B}{.Math.{}_B{t}_{B,\mathrm{Si}}.Math.}_x\\ .Math.& .Math.\\ {\left({}_{\mathrm{Si}}{p}_J^0\right)}^T{R}_{\mathrm{Si},B}& -{\left({}_{\mathrm{Si}}{p}_J^0\right)}^T{R}_{\mathrm{Si},B}{.Math.{}_B{t}_{B,\mathrm{Si}}.Math.}_x\end{array}\right].Math.\left[\begin{array}{c}{}_B{v}_{W,B}\\ {}_B{?}_{W,B}\end{array}\right]$$\left(\begin{array}{c}-d_1\\ .Math.\\ -d_J\end{array}\right)=M.Math.\left[\begin{array}{c}{}_B{v}_{W,B}\\ {}_B{?}_{W,B}\end{array}\right]$

[0081] In this case, the multiplicity of measurement data sets from each of the plurality of sensors can each include a plurality of measurement data sets. By way of example, if, for example, four sensors are provided, each sensor could incorporate 50 to 150 measurement data sets into the equation system, i.e., a total of 200 to 600 measurement data sets. In this case, the index Si is used in a generic sense, i.e., in each row of the equation system it is to be understood as the respective sensor or its sensor reference system to which the measurement data set is assigned. Therefore, measurement data sets from different sensors can be included in the equation system.

[0082] This equation system can be solved by way of a regression method. As above, therefore, a functional F is specified that is dependent on the differences or Doppler velocity deviations (d.sub.j?{circumflex over (d)}.sub.j) between the measured Doppler velocities d.sub.j and the estimated Doppler velocities {circumflex over (d)}.sub.j and is minimized by the solution (in this case again by squared distances, by way of example):

[00012]$F=F\left[\left(d_1-{\stackrel{?}{d}}_1\right)\left(d_j-{\stackrel{?}{d}}_j\right),.Math.,\left(d_J-{\stackrel{?}{d}}_J\right)\right]?\underset{j}{.Math.}{\left(d_j-{\stackrel{?}{d}}_j\right)}^2$

[0083] In this case too, different measurement data sets can be weighted differently with weights or parameter weights g.sub.j on the basis of additional parameters of the measurement data sets:

[00013]$F=F\left[g_1\left(d_1-{\stackrel{?}{d}}_1\right),.Math.,g_j\left(d_j-{\stackrel{?}{d}}_j\right),.Math.,g_J\left(d_J-{\stackrel{?}{d}}_J\right)\right]?\underset{j}{.Math.}{g_j^2\left(d_j-{\stackrel{?}{d}}_j\right)}^2$

[0084] In addition, individual measurement data sets can remain disregarded in equation system (5) on the basis of additional parameters.

[0085] In this context, and as above, the expression estimated Doppler velocities refers to the fact that they can be calculated for a specific linear velocity vector .sub.Bv.sub.W,B (estimated as part of the regression method or the optimization) and a specific angular velocity vector .sub.B?.sub.W,B using the rows on the left-hand side of the above equation system (5). In the regression method, a linear velocity vector .sub.Bv.sub.W,B and an angular velocity vector .sub.B?.sub.W,B are then determined such that the respective functional is optimized or minimized. This represents a determination or estimation of the movement state of the body.

[0086] FIG. 3 shows a diagram of the sequence of a plurality of determination and measurement times over time. In the diagram, a plurality of determination times T.sub.0, T.sub.1, T.sub.3 are plotted on a time axis t; these represent times for which the movement state of the rigid body is to be determined or estimated. In general, the determination time is not the same as a calculation time at which the movement state is determined or calculated.

[0087] The corresponding movement states are denoted by Z.sub.0=Z(T.sub.0), Z.sub.1=Z(T.sub.1), Z.sub.2=Z(T.sub.2). A movement state Z(T.sub.k) at one time T.sub.k is characterized by the linear velocity vector .sub.Bv.sub.W,B(T.sub.k) and the angular velocity vector .sub.B?.sub.W,B(T.sub.k) of the rigid body at the relevant time T.sub.k.

[0088] The determination times can, as shown, be arranged at a regular distance from one another, an interval ?T between two successive determination times being specified as ?T=T.sub.k+1?T.sub.k. The inverse 1/?T of the interval ?T can be regarded as the determination frequency. More generally, however, determination times arranged at an irregular distance from one another are also possible.

[0089] In addition, a plurality of measurement times are indicated in the figure, i.e., times at which at least one sensor captures objects. At each measurement time, the sensor will determine a multiplicity of individual measurement data sets, each of which includes at least one Doppler velocity, an azimuth angle, and an elevation angle of a captured object. In addition, each of the measurement data sets can include additional parameters.

[0090] Besides the Doppler velocity, the azimuth angle, and the elevation angle, each of the measurement data sets includes a measurement time at which these were measured. Shown by way of example are measurement times t.sub.?1, t.sub.0, t.sub.1, t.sub.2, t.sub.3, t.sub.4, t.sub.5 for two sensors S1 (measurement times t.sub.?1, t.sub.1, t.sub.3, t.sub.4) and S2 (measurement times t.sub.0, t.sub.2, t.sub.5), where t.sub.?1 in this case randomly coincides with T.sub.0. The measurement times of the sensors can be independent of one another and/or independent of the determination times. In addition, where applicable measurement frequencies of different sensors can be different from one another and/or different from the determination frequency. In the figure, for example, the measurement frequency of the sensor S2 is the same as the determination frequency whereas the sensor S1 has a higher measurement frequency. Preferably, the measurement times of different sensors are different from one another so as to avoid interference.

[0091] A determination frequency or an average number of determination times per second can be in the range from 5-10 Hz, for example. The measurement frequency of a sensor or an average number of measurement times of the sensor per second can be in the range from 10-20 Hz, for example. The measurement frequencies (or average numbers of measurement times per second) of the sensors are preferably greater than the determination frequency (or an average number of determination times per second).

[0092] In principle, the movement state is determined or estimated by a regression method or a regression as described above, i.e., in accordance with the weak-coupling approach or the strong-coupling approach. In the process, measurement data sets having measurement times which occurred temporally close to the determination time, in particular within particular time limits around the determination time, are incorporated. By way of example, all the measurement data sets that occurred within a predetermined interval dT, i.e.: |t.sub.j?T.sub.k|<dT can be incorporated; in this case, asymmetric limits having a lower interval d.sub.uT and an upper interval d.sub.oT are also possible: ?d.sub.uT<t.sub.j?T.sub.k<d.sub.oT. Another option is to incorporate all the measurement data sets having measurement times t.sub.j that are between the previous determination time T.sub.k?1 and the next determination time T.sub.k+1: T.sub.k?1<t.sub.j<T.sub.k+1 or expressed as the upper/lower interval, d.sub.oT=T.sub.k+1?T.sub.k, and d.sub.uT=T.sub.k?T.sub.k?1. In the case of determination times arranged at a regular distance from one another: d.sub.oT=d.sub.uT=?T.

[0093] In general terms, therefore, measurement data sets having measurement times within a determination period are incorporated into the determination of the movement state for the determination time T.sub.k. The determination period is determined by a lower and an upper time limit; for example, according to the above configuration, the interval [T.sub.k?d.sub.uT; T.sub.k+d.sub.oT] can be a determination period. Preferably, the determination time T.sub.k is within the determination period. In addition, it is generally possible for the determination time T.sub.k to be outside the determination period, i.e., that d.sub.uT<0 or d.sub.oT<0 in the above example.

[0094] The movement state for a determination time is determined at least at one calculation time, which is generally different from the determination time for which the movement state is determined, so as to be able to also incorporate measurement data sets having measurement times after the determination time.

[0095] By way of example, the figure indicates two calculation times ?.sub.1, ?.sub.2 at which the movement state Z.sub.1 for the determination time T.sub.1 can be determined, the first calculation time ?.sub.1 being between two measurement times t.sub.2, t.sub.3 and the second calculation time ?.sub.2 being after the measurement time t.sub.3, in this case between the measurement time t.sub.3 and the determination time T.sub.2 for example. Equally, however, it could also coincide with said measurement time. For the above-described time limits around the determination time, the calculation time represents an upper limit; in other words, the quantity of measurement data sets to be taken into account is restricted such that only those that additionally satisfy the condition t.sub.j??.sub.b are taken into account (?.sub.b denotes a calculation time), i.e., those whose measurement time is at or before the calculation time. To determine the movement state Z.sub.1 in FIG. 3, by way of example, the measurement data sets having measurement time t.sub.3 are not taken into account at the determination time ?.sub.1 whereas the measurement data sets having measurement time t.sub.3 are taken into account at the determination time ?.sub.2.

[0096] The movement state for a determination time can be determined multiple times, at a plurality of different calculation times. For instance, a determination could be triggered whenever new measurement data sets having measurement times within the time limits are available for the determination time. In this respect, it is also possible to do so only when the measurement time is after the determination time. In principle, however, it is also possible for a calculation time to be before the determination time for which the movement state is determined. By way of example, the measurement data sets having the measurement times t.sub.2 and t.sub.3 could be used at the determination time ?.sub.2 in order to carry out a determination or estimation for the movement state Z.sub.3 at the determination time T.sub.3 (not shown in the figure). It is also possible to arrange the calculation times at a regular distance from one another in time. In addition, the calculation times could coincide with the determination times, the movement state for the previous determination time being determined in each case. In particular, the manner in which the calculation times are determined is also dependent on the available computing power.

[0097] To take account of the fact that the measurement times do not coincide with the determination times, the measurement data sets in the functional F that is to be minimized are weighted on the basis of the measurement time and the determination time. Therefore, weights w.sub.j, which can be referred to as time weights, are provided and are dependent on the difference between the measurement time t.sub.j and the determination time T.sub.k: w.sub.j=w.sub.j(t.sub.j?T.sub.k); preferably the weights are dependent on the magnitude of the difference: w.sub.j=w.sub.j(|t.sub.j?T.sub.k|). These weights should always be either positive or exactly zero.

[0098] The functional dependency of the weights should be selected such that when the difference between the measurement time t.sub.j and the determination time T.sub.k is exactly zero, the weight assumes its maximum value: w.sub.j(0)=w.sub.max, and decreases monotonically, preferably strictly monotonically, as the magnitude increases until the value zero is reached, the value zero preferably being reached when the upper limit T.sub.k+d.sub.oT or lower limit T.sub.k?d.sub.uT of the measurement times being taken into account is reached: w.sub.j(T.sub.k+d.sub.oT?T.sub.k)=w.sub.j(d.sub.oT)=0=w.sub.j(T.sub.k?d.sub.uT?T.sub.k)=w.sub.j(?d.sub.uT). The maximum value can be selected as any positive number, for example exactly one: w.sub.max=1. The maximum value w.sub.max is the same for all the measurement data sets (and, where applicable, for other additional measurements; see below).

[0099] In particular, the following may apply:

[00014]$w_j=\{\begin{array}{ccc}{w}_{\max }f\left(1-\frac{t_j-T_k}{d_{o}T}\right)& \text{;for}& t_i?\left[T_k;T_k+d_{o}T\right]\\ w_{\max }f\left(1-\frac{T_k-t_j}{d_{u}T}\right)& \text{;for}& t_j?\left[T_k-d_{u}T;T_k\right]\\ 0& \text{;for}& t_j.Math.\left[T_k-d_{u}T;T_k+d_{o}T\right]\end{array}$

[0100] More generally, the above linear decrease as the distance increases can also be modified by a function f that increases monotonically, preferably strictly monotonically, as the argument increases:

[00015]$w_j=\{\begin{array}{ccc}{w}_{\max }f\left(1-\frac{t_j-T_k}{d_{o}T}\right)& \text{;for}& t_i?\left[T_k;T_k+d_{o}T\right]\\ w_{\max }f\left(1-\frac{T_k-t_j}{d_{u}T}\right)& \text{;for}& t_j?\left[T_k-d_{u}T;T_k\right]\\ 0& \text{;for}& t_j.Math.\left[T_k-d_{u}T;T_k+d_{o}T\right]\end{array}$

[0101] For example, a quadratic function f(x)=x.sup.2 could be used.

[0102] In the functional F that is to be minimized in the regression method, the measurement data sets are then weighted accordingly, i.e., the differences (d.sub.j?{circumflex over (d)}.sub.j) between measured and estimated Doppler velocities are multiplied in the functional by the relevant time weight w.sub.j of the measurement data set; in other words, w.sub.j(d.sub.j?{circumflex over (d)}.sub.j) is used in the functional:

[00016]$F=F\left[w_1\left(d_1-{\stackrel{?}{d}}_1\right),.Math.,w_j\left(d_j-{\stackrel{?}{d}}_j\right),.Math.,w_J\left(d_J-{\stackrel{?}{d}}_J\right)\right]?\underset{j}{.Math.}{w_j^2\left(d_j-{\stackrel{?}{d}}_j\right)}^2$

[0103] If additional weighting is carried out on the basis of additional parameters of the measurement data sets, the differences are additionally multiplied by the relevant parameter weight g.sub.j; in other words, g.sub.jw.sub.j(d.sub.j?{circumflex over (d)}.sub.j) is used in the functional:

[00017]$F=F\left[g_1{w}_1\left(d_1-{\stackrel{?}{d}}_1\right),.Math.,g_j{w}_j\left(d_j-{\stackrel{?}{d}}_j\right),.Math.,g_J{w}_J\left(d_J-{\stackrel{?}{d}}_J\right)\right]?\underset{j}{.Math.}{g}_j^2{w_j^2\left(d_j-{\stackrel{?}{d}}_j\right)}^2$

[0104] In both cases here, the preferred quadratic functional in the arguments (i.e., in the weighted differences) was again stated as an example. This functional can be used for both the solution (in terms of the regression method) to equation system (5) and the solution to equation system (2).

[0105] Each row of equation system (5) or equation system (2) represents a condition for the linear velocity vector and the angular velocity vector or for the relevant velocity of the sensor. Since the rows of each equation system correspond to measurement data sets, each measurement data set can be deemed a condition.

[0106] To determine the movement state for a determination time, one set of conditions, i.e., one condition set, is selected from the set of all the measurement data sets at a calculation time and is used for determining the movement state. These are in particular the measurement data sets having a measurement time that is within an interval [T.sub.k?d.sub.uT; T.sub.k+d.sub.oT] around the determination time T.sub.k (and whose measurement time is before or at the calculation time).

[0107] In this case, it is possible to add further conditions not originating from the measurement data sets to the set of conditions. These may be measurements by other measurement instruments and/or predetermined constraints on the movement of the body.

[0108] Preferably, an angular rate sensor or gyroscope is provided, which is arranged in particular at the origin of the body reference system. This then results in the following as an additional condition or equation for equation system (5):

[00018]${}_B{\widetilde{?}}_{W,B}=\left[\begin{array}{cc}0& I\end{array}\right]\left[\begin{array}{c}{}_B{v}_{W,B}\\ {}_B{?}_{W,B}\end{array}\right]$

where .sub.B{tilde over (?)}.sub.W,B is the angular velocity measured by the angular rate sensor and I is the three-dimensional unit matrix.

[0109] A constraint on the movement of the body (for instance a motor vehicle moving in a plane) could, for example, be as follows (in this case formulated as conditions for individual components of the velocity vector and angular velocity vector):

.sub.B.sub.W,B=0, .sub.B.sub.W,B=0, .sub.B.sub.W,B=0

[0110] Each of these conditions is taken into account in the functional of the regression method using a respective (additional-condition) weight. The magnitude of the weight determines the extent to which the additional condition or constraint is taken into account. In addition, in the case of measurements, for instance the angular velocity measurement by the angular rate sensor, a (time) weight that is dependent on the measurement time in relation to the determination time can be provided. In particular, such time weights can be determined as described in the context of the measurement times of the sensors.

[0111] For example, in the case of angular velocity measurements by an angular rate sensor, an additional functional F.sub.G can be added to the above functional F to obtain a total functional: F.sub.total=F+F.sub.G, where

F.sub.G=F.sub.G[Gw.sub.G(|.sub.B{tilde over (?)}.sub.W,B?.sub.B{tilde over ({circumflex over (?)})}.sub.W,B|)]?G.sup.2w.sub.G.sup.2|.sub.B{tilde over (?)}.sub.W,B?.sub.B{tilde over ({circumflex over (?)})}.sub.W,B|.sup.2 [0112] .sub.B{tilde over ({circumflex over (?)})}.sub.W,B denotes the estimated angular velocity vector and |?| denotes the vector norm; [0113] G is the weight on the basis of which the measurements taken by the angular rate sensor are taken into account; [0114] w.sub.G is the time weight determined on the basis of the measurement time of the angular rate sensor.

[0115] Since the angular rate sensor typically has greater accuracy than sensors, and since generally a high number of measurement data sets and relatively few angular rate sensor measurements are available, the weight G is preferably selected to be much higher than the corresponding weighting of the sensor measurements. In the above formulas for the functional F, this corresponding weighting of the sensor measurements is in each case 1 or formulated otherwise; since a constant (positive) factor in the functional is not relevant for the optimization but merely represents a normalization, the functional for the measurement data sets can be normalized such that said corresponding weighting of the sensor measurements is exactly 1. The weight G is thus preferably selected to be much higher than 1: G>>1; e.g., G>10 or more, preferably G>100. Equally, the weight G can have an upper limit, for example G<1000.

[0116] As already mentioned above, a robust regression method is preferably used in order to minimize the effect of outliers, which are specified in particular by non-static objects. Preferably, an iteratively reweighted least squares (IRLS) method can be used.

[0117] In this case, the method starts from the above quadratic functional, and the quadratic deviation between estimated and measured Doppler velocities is weighted in the functional for each measurement data set or for each condition using an iteration weight h.sub.j,k, where k denotes an iteration parameter and j denotes the measurement data set as before. By way of example, this then results in the following (the optional time weights w.sub.j and parameter weights g.sub.j being taken into account in this case):

[00019]$F_{\mathrm{IRLS}}=\underset{j}{.Math.}{h}_{j,k}{g}_j^2{w_j^2\left(d_j-{\stackrel{?}{d}}_j\right)}^2$

[0118] For specific iteration weights h.sub.j,k, said functional is minimized using the least squares method. On the basis of the discovered solution, i.e., on the basis of the discovered {circumflex over (d)}.sub.j, new iteration weights h.sub.j,k+1 are determined. In this case, iteration weights for which the discovered estimated Doppler velocities have a large deviation from the measured Doppler velocities are reduced in relation to those with a small deviation. A particular value can be specified as a starting value h.sub.j,0 for the iteration weights, for instance 1: h.sub.j,0=1, for all j.

[0119] Up to now, the azimuth angles ? and elevation angles ? included in the measurement data sets and included in the equation systems to be solved, in each case on the left-hand side, have been assumed to be exact. Generally, however, as measured values they represent values containing errors. To take this into account, an error-in-variables regression can preferably also be used.

[0120] In this case, the process can be as follows. First, the movement state is determined or estimated using the above-described strong-coupling approach. Next, using the determined movement state, an error-in-variables regression is carried out to optimize the azimuth angle ? and/or elevation angle ?. Using the determined optimized azimuth angle ? and/or elevation angle ?, the movement state is determined or estimated again using the strong-coupling approach, and the azimuth angle ? and/or the elevation angle ? are optimized again on that basis. This is repeated until the results converge, for example until successive determined movement states no longer change within a predetermined tolerance and/or until a predetermined maximum number of repetitions is reached. Overall, this approach again represents a regression analysis.

[0121] If the movement states of the rigid body are determined for a plurality of successive determination times, then by integrating the linear velocity vectors .sub.Bv.sub.W,B and/or the angular velocity vectors .sub.B?.sub.W,B over time it is possible to calculate a relative position and/or orientation of the body at an end determination time T.sub.End relative to a starting position and/or a starting orientation that was present at a starting determination time T.sub.Start. Since the movement state is determined only for discrete determination times, the time integral is typically given as a sum.

[0122] FIG. 4 shows a flowchart of a preferred specific embodiment of the present invention.

[0123] In step 102, measurement data sets are obtained by one or more sensors. Likewise, further measurement results can be captured, in particular angular rate measurement results obtained by an angular rate sensor or a gyroscope. The capturing can be carried out at a plurality of measurement times; in particular, each sensor can take measurements and capture the measurement results continuously, for instance at a specific measurement frequency.

[0124] In step 104, the measurement data sets and, where applicable, further measurement results are transmitted to an arithmetic logic unit. In this case, the measurement data sets are sent by the sensors to the arithmetic logic unit and received thereby. Likewise, the further measurement results, where applicable, are sent by the relevant measurement sensors to the arithmetic logic unit and received thereby. In both cases, the sensors can be connected to the arithmetic logic unit either directly or indirectly; a buffer can also be provided in order to carry out an evaluation (i.e., to determine the movement state) at a later time. The arithmetic logic unit can be found either on or in the rigid body whose movement state is to be determined. Equally, the arithmetic logic unit can be remote from the body, e.g., can be a remote computer or server. The measurement results can be transmitted to the arithmetic logic unit via a communication network (for example also wirelessly) or on suitable data media (e.g., when evaluating the results at a later time).

[0125] Steps 102 and 104 together can be regarded as the providing step. Providing can also be carried out in a different way, for example if the measurement data sets and the further measurement results are already available as stored data or if simulated measurement data sets are used. In this respect, steps 102 and 104 are preferred steps.

[0126] In step 106, at least one set of conditions is created from the measurement data sets and, where applicable, further measurement results. The process here is as described above in relation to FIG. 3. Therefore, for a determination time for which the movement state is to be determined or calculated, measurement data sets and, where applicable, further measurement results having measurement times within a determination period are incorporated into a set of conditions. This can be done once or multiple times at corresponding calculation times.

[0127] In preferred step 108, weights are determined, by which the measurement data sets and, where applicable, further measurement results incorporated in the set of conditions are weighted in the functional used in the regression analysis. As described above, these are time weights and/or parameter weights and/or, where applicable, weights by which the further measurement results are weighted relative to the measurement data sets. These weights are determined before the actual regression method is carried out and remain unchanged while the regression method is carried out (for one calculation time). In addition, further weights that are different from those above and, where applicable, are changed over a plurality of iterations of the regression method can be used during the regression analysis, for example the above-mentioned iteration weights in the case of IRLS. Filtering can also be carried out here, in which certain measurement data sets are excluded from a set of conditions on the basis of additional parameters of the measurement data sets.

[0128] Next, on the basis of the at least one set of conditions, a regression analysis is carried out, in which the movement state (linear velocity vector and angular velocity vector) of the body is determined.

[0129] On the one hand, step 110 can be carried out in accordance with the strong-coupling approach; in other words, the Doppler velocities are correlated directly with the movement state in the regression analysis (equation system (5)).

[0130] On the other hand, the weak-coupling approach can be used. In this case, in step 112 the sensor velocity vectors are first determined by correlating them with the Doppler velocities in a regression method. Next, in step 114, the movement state is calculated from the sensor velocity vectors in a further regression.

[0131] As explained above in relation to step 108, in steps 110, 112, and 114 further, variable weights that are different from the time weights, parameter weights, and weights by which further measurement results are weighted can be used, where applicable, as part of the regression method being used in each case (for instance as part of IRLS).

[0132] In a preferred step 116, it can be determined whether the movement state of the vehicle is to be determined for a further determination time and/or on the basis of a different determination period, for instance if new measurement data sets are now available. If this is the case, the method can jump back to step 106 (arrow 118), in which at least one new set of conditions is determined for the further determination time and/or on the basis of a different determination period. Irrespective of this, the method can continue with the optional step 120 for ready-calculated movement states.

[0133] In preferred step 120, the calculated movement state or states can be output and/or forwarded for further processing. In particular, if movement states have been determined for a plurality of successive determination times, the movement states can be integrated over time in order to determine a relative position and orientation of the body between a starting state at an earlier determination time and an end state at a later determination time.