Kalman filter for estimating the states of a motorized device

Abstract

Kalman filter for estimating the states of a motorized device The present description relates to a method for estimating, by a processor, a vector comprising values ​​of physical states of a motorized device at an instant, the physical states comprising at least one indication of the position of the device, the discrete-time evolution of the states of the device being described by a system of recursive dynamic equations, the method comprising: - the normalization of the system of dynamic equations; - the factorization of a descriptor Kalman filter; and - the calculation, by the processor connected to the sensor and on the basis of a vector comprising the measurements carried out by the sensor, of an estimate of the vector on the basis of the factored descriptor Kalman filter. Figure for the abstract: Fig. 4

Description

Title of the invention: Kalman filter for estimating the states of a motorized device Technical field

[0001] The present description relates generally to methods and devices for estimating the physical states, in particular the position and speed, of a motorized device, by applying a Kalman filter. Prior art

[0002] Kalman filters are used in a wide range of technological fields. In particular, Kalman filters make it possible to estimate states of a dynamic system whose measurements are incomplete and / or noisy. For example, the application of a Kalman filter makes it possible to estimate the position and / or the speed and / or the acceleration of a motorized object, such as for example a geared motor or a vehicle such as an airplane or even a satellite.

[0003] However, Kalman filters are generally not numerically stable. Thus, when a processor executes estimates by applying a Kalman filter, errors, for example rounding errors, accumulate and propagate during the estimation steps. The estimates are then distorted and / or the associated covariance matrices lose their theoretical property of definite positivity. Commands generated on the basis of these estimates for controlling the motorized object are then also distorted, leading to undesired behaviors of the object, such as for example overconsumption or material damage.

[0004] There is a need for a technical solution to improve numerical stability when programming a Kalman filter, particularly for a system described by a descriptor state model. Summary of the invention

[0005] One embodiment provides a factored Kalman filter for a system described by a descriptor state model. In particular, the descriptor state model is obtained by scaling the manipulated quantities.

[0006] One embodiment provides a method for estimating, by a processor, a vector 'U + i comprising values ​​of physical states of a motorized device at a time ke, the physical states comprising at least one indication of the position of the device, the discrete-time evolution of the states of the device being described by a system of recursive dynamic equations of the form = + + ¾ , where the variable u represents a control vector yk=Cxk+vk generated by a control circuit of the motorized device, the variable H is a random vector, modeling noise and disturbances external to the device, having a covariance matrix of a matrix Q; the variable y comprises measurements, carried out by a sensor, of at least one of the physical states and / or the indirect measurement of at least one of the physical states; the variable v is a random vector, modeling noise and measurement errors, having a covariance matrix of a matrix R; and the matrices A, B and C are respectively matrices of evolution of the physical states, of the control inputs and of the measurements, the method comprising: - the normalization of the system of dynamic equations, the normalization including the search, by the processor, for a matrix and a matrix W2 such that the matrices A = WjA, B ​​— and C = W2C are bounded, and their storage in a memory; - the factorization of a descriptor Kalman filter having, for the moment, a covariance matrix P^, the factorization including the calculation of a triangular matrix Uk per block such that , where is the covariance matrix associated with and where Rk is the covariance matrix associated with and the storage in the memory of at least one of the matrices composing the block matrix Uk; and - the calculation, by the processor connected to the sensor and on the basis of the vector yk comprising the measurements carried out by the sensor, of an estimate x^^j of the vector on the basis of the factored descriptor Kalman filter.

[0007] According to one embodiment, the normalization of the system of dynamic equations further comprises, before the search for the matrices and W2, the search, by the processor, for matrices 7^, TB and Tc such that, for each instant, the variables xk ~ PAxk, yk ~ PByk and ük = TCuk are bounded and in which the search for the matrices and W2 is carried out so that the matrices A = VFj^AT*1, B = and C = w2tcct2 are bounded.

[0008] According to one embodiment, the covariance matrix Pj^ is guaranteed to be numerically symmetric and positive definite and in which the calculation of the estimate x^^, comprises the execution, by the processor, of instructions stored in the memory, the instructions programming the calculation of + Qk) + P^k ) Rk ÿfc+1 = W$k+iet where ^W+l depends on the matrix Uk.

[0009] According to one embodiment, the matrix Pk+\^+i is such that Pk+^+ï = Vk^VkT^ od Vk is a matrix such that V k . 0 . , where r6 is an orthogonal matrix and where and Us are blocks of the triangular matrix Uk.

[0010] According to one embodiment, the matrix is ​​such that ig \ T r; — W and \ IJc / I matrix U^k is such that / rr \1 jj _ p?, where the matrix U]k is equal to and the matrix is ​​the Cholesky factor of the matrix Rk. ÀP^Â + Q

[0011] According to one embodiment, the matrices and W2 are diagonal matrices

[0012] According to one embodiment, at least one of the matrices A, B or C depends on the instant tk.

[0013] According to one embodiment, at least one of the matrices A, B and / or C is obtained by linearization of nonlinear state model.

[0014] According to one embodiment, the search, by a processor, for at least one of the matrices Wi and W2 is carried out by applying an optimization algorithm.

[0015] According to one embodiment, the estimate X£+yc+! is a vector of size n, n being an integer, and the vector is a vector of size m, m being an integer less than or equal to n.

[0016] One embodiment provides a system comprising: - a motorized device, the evolution of one or more physical states of which is described by a system of recursive dynamic equations of the form xk+^Axk+Buk + nk , the variables xk and x*+ i being respectively vectors yk=Cxk+vk describing one or more physical states of the motorized device at a time tk and a successive time ^+i, the variable uk representing a control vector generated, for the time tk, by a control circuit of the system, the variable being a random vector, modeling noise and disturbances external to the device for the time tk, having as covariance matrix a matrix Qk; the variable yk comprising measurements, carried out by a sensor, of at least one of the physical states and / or at least one indirect measurement of one of the physical states, at the time tk; the variable vk is a random vector, modeling noise and measurement errors for the time tk, having as covariance matrix a matrix Rk; - a memory in which matrices A, B and C, matrices W ] and W2 are stored, such that the matrices A “ WjA, B ​​= W^B and C = W2C are bounded, and to store sets of matrices g and Rk, where for each instant k^N is kk the covariance matrix associated with and where Rk is the covariance matrix associated with the memory further storing, in association at each instant 4 a covariance matrix P^k, such that r ... -T AP^A +Qk 0 W1' , a 0 Rk c wf cT h 1 0 i block triangular Uk matrix; - the control circuit configured to generate the control data uk for each instant 4; - the sensor configured to generate, at each instant tk, the vector yk comprising the measurement of at least one of the physical states and / or the indirect measurement of at least one of the physical states of the motorized device, of the motorized device at the instant tk, the sensor being further configured to transmit the generated vector to a processor; - the processor configured for: generate a vector ÿk based on the received vector yk and the matrix W2; generating a vector ük based on the control data generated by the control circuit and the WT matrix; generate an estimate of the physical states of the motorized device at time ^+i based on the matrices A, B, C, ^b ^2, Q^, Rk and P / ^ order the storage, in the memory of the covariance matrix Pk+ty+ï of the estimate £w+r

[0017] According to one embodiment, the control circuit is configured to generate control data uk+\ based on the estimate x^+^+r

[0018] According to one embodiment, the memory further stores matrices T^ TB, T c such that, for each instant, the variables xk = T Axk, ÿk=TByk and ük ~ Tcuk are bounded and in which the matrices Wj and W2 are such that the matrices A = W^ATA B = WlTaBT^ and C = W2TcCJ^ are bounded, the processor being further configured to: - generate the vector ÿk based on the matrix Te; and - generate the vector ük based on the matrix TB. Brief description of the drawings

[0019] These characteristics and advantages, as well as others, will be explained in detail in the following description of particular embodiments given without limitation in relation to the attached figures among which:

[0020] [Fig.l] is a block diagram showing an example of a system configured for estimating one or more states of a motorized object;

[0021] [Fig.2A] illustrates an example of a motorized object;

[0022] [Fig.2B] is a graph illustrating measurements made by a sensor on a motorized object;

[0023] [Fig.3] is a block diagram illustrating variables involved in the evolution of the states of a motorized object;

[0024] [Fig.4] illustrates an example of the steps in a method for estimating the physical states of a motorized object by applying a Kalman filter;

[0025] [Fig. 5] is a flowchart illustrating steps of implementing the method of [Fig. 4] according to an embodiment of the present description; and

[0026] [Fig.6] is a block diagram illustrating an example of a system comprising a geared motor according to an embodiment of the present disclosure. Description of the embodiments

[0027] The same elements have been designated by the same references in the different figures. In particular, the structural and / or functional elements common to the different embodiments may have the same references and may have identical structural, dimensional and material properties.

[0028] For the sake of clarity, only the steps and elements useful for understanding the described embodiments have been represented and are detailed. In particular, the methods for normalizing the state model of a dynamic system are not described and are known to those skilled in the art.

[0029] Unless otherwise specified, when referring to two elements connected to each other, this means directly connected without intermediate elements other than conductors, and when referring to two elements connected (in English "coupled") to each other, this means that these two elements can be connected or be connected by means of one or more other elements.

[0030] In the following description, when reference is made to absolute position qualifiers, such as the terms "front", "back", "top", "bottom", "left", "right", etc., or relative position qualifiers, such as the terms "above", "below", "upper", "lower", etc., or to orientation qualifiers, such as the terms "horizontal", "vertical", etc., reference is made unless otherwise specified to the orientation of the figures.

[0031] Unless otherwise specified, the expressions "about", "approximately", "substantially", and "of the order of" mean to within 10%, preferably to within 5%.

[0032] Figure 1 is a block diagram showing an example of a system 100 configured for estimating one or more physical states of a motorized object 102 (DEV). For example, the object 102 is an airplane, a satellite, a motorized element of a machine, such as a factory machine, a robotic arm, etc. The physical states of the object 102 correspond for example to its position, its speed and / or its acceleration. The physical states of the object are, for example, described by a vector of dimension fl, n being an integer greater than or equal to 1. In the example where the estimated states are the position, velocity and acceleration of an object moving in a three-dimensional space, n is equal to 9. In another example, the object is a geared motor or an actuator, belonging for example to a larger device such as for example an automobile. The geared motor is for example a windshield wiper motor. In this example, the states of the object are for example an angle, an angular velocity and / or an angular acceleration.

[0033] The states of the object are for example measured by one or more sensors 104 (SENSOR). For example, the sensor(s) are radars configured to measure the position and / or speed of an aircraft, a satellite, etc. In another example, the sensor(s) are for example integrated into the device comprising the object 102. For example, the sensor(s) are configured to measure an angle or a position in a one-dimensional space. In some examples, the sensor(s) are connected, for example by wire, to the object 102.

[0034] The sensor(s) 104 are for example further connected to a circuit 106. By way of example, the circuit 106 comprises a processor 108 (CPU), such as a central processing unit or a microcontroller, configured to calculate the states of the object 102 on the basis of measurements made by the sensor(s) 104 and transmitted to the circuit 106. By way of example, the circuit 106 further comprises a non-volatile memory 110 (NV MEM) and / or a volatile memory 112 (RAM) connected to the processor 108 via a bus 114. By way of example, the volatile memory 112 is a random access memory (in English “Random Memory Access”) configured to receive, and store, the measurements transmitted by the sensor(s) 104.

[0035] The system further comprises a control circuit 116 (CTRL) configured to transmit, to the object 102, control input data. For example, the control circuit 116 is configured to generate the control input data based on the estimates of the physical states of the object, for example provided by the circuit 106, such as by the processor 108. For example, the circuits 106 and 116 are connected by a wired communication link. In another example, the circuits 106 and 116 are connected by a wireless communication link, for example via radio frequency (RF) signals, and in this case, the circuit 106 comprises, for example, an antenna (not shown) configured to transmit these RF signals, and the control circuit 116 comprises, for example, an antenna (not shown) configured to receive these RF signals.In another example, the control circuit 116 and the circuit 106 are integrated into a single device.

[0036] As an example, the control data is transmitted from the control circuit 116 to the object 102 in a wired manner. In another example, the data control signals are transmitted from the control circuit 116 to the object 102 wirelessly, for example via RF signals. The control circuit 116 then comprises, for example, an antenna (not shown) configured to transmit these RF signals, and the object 102 comprises, for example, an antenna (not shown) configured to receive these RF signals.

[0037] [Fig.2A] illustrates an example of a motorized object. In the example illustrated by [Fig.2A], the object takes the form of an airplane 200. The example of an airplane is of course not limiting.

[0038] By way of example, a sensor 202 is configured to measure the position of the aircraft 200. In the example illustrated by [Fig.2A], three successive positions of the aircraft 200 are illustrated. By way of example, the three positions of the aircraft are positions measured by the sensor 202 at regular time intervals. By way of example, the time interval is a fixed value and is of the order of a few seconds.

[0039] By way of example, the sensor 202 transmits each measurement of the position of the aircraft 200 to a processing unit (not illustrated) configured to estimate, in addition to the next position of the aircraft, for example its speed and its acceleration.

[0040] In another example, the sensor 202 is further configured to measure the speed of the aircraft 200.

[0041] [Fig.2B] is a graph 204 illustrating measurements made by the sensor 202 on the aircraft 200.

[0042] The graph 204 comprises for example a plurality of speed measurements (State) of the aircraft 200 carried out at regular time intervals (Time(k)). For example, at a time h), respectively at times lj, ^2 and ^N, a measurement zo, respectively measurements ci, and zn. of the speed of the aircraft 200 are carried out. A curve 208 represents the actual speed of the aircraft 200. For example, the curve 208 is constant, meaning that the actual speed of the aircraft 200 is constant. However, the measurements carried out by the sensor 202 fluctuate around the actual value. Indeed, errors in the measurements carried out by the sensor 202 occur and are for example linked to noise and / or external disturbances. Furthermore, the sensor 202 is for example not 100% reliable and the measurements carried out are for example carried out with a margin of error depending for example on the quality of the sensor 202.

[0043] According to one embodiment, estimates of states, such as for example, the position, speed and / or acceleration of an object are made following the application of a Kalman filter to the measurements made by the sensor(s). The application of a Kalman filter has the effect of reducing errors, due to noise, disturbances and measurement errors, in the estimation of one or more states, such as the position, speed and / or acceleration, of an object, and of estimating unmeasured observable states.

[0044] [Fig.3] is a block diagram illustrating variables involved in the evolution of the states of an object, for example a motorized object.

[0045] As an example, at a time the physical states of the object are described by a number e N * of variables x^t^, x2(tk), , Xn(tk) (State variables at the time k). For example, n = 9 and the variables xt ( tk ), X2 ( tk ) and x3 ( tk ) are the coordinates describing the position of the object in a three-dimensional space. Similarly, the variables x^( tk ), ) and x6( describe for example the linear velocity of the object in three-dimensional space, and the variables X7(tk), x8() ct -^9(the linear acceleration of the object in three-dimensional space. This example is given for illustrative purposes, and other states describing the object can of course be considered depending on the application domain considered. For example, the space in which the object evolves can vary and be one-dimensional, or two-dimensional. Similarly, instead of describing a position, a velocity and an acceleration, the states can describe other types of position data, such as an angle, an angular velocity and / or an angular acceleration, etc. Similarly, state variables can describe other physical quantities, such as the torque applied to a motor shaft of an electric motor, the current and / or the voltage applied to the terminals of an electric motor, etc.

[0046] For example, at time tk, control data ( tk ) and ( tk) (Control inputs) are applied to the object. For example, the control inputs are used to control a change of direction, slowing down, or accelerating of the object. It is of course possible for the number of control inputs to differ from two. For example, in another embodiment, only one control input is applied to the object. In another example, more than two control inputs are applied to the object.

[0047] As an example, the evolution of the physical states of the object is also subject to noise and / or disturbances (Noise and disturbances). In the example where the object is an airplane, the noise and disturbances may, for example, be of meteorological origin. These noises and disturbances are for example represented by a vector q(tk), of dimension i.e. the same dimension as the number of physical state variables. The evolution of the state variables, also called physical “states” of the object, i.e. the states at time 4+i which are denoted jq(q+1), x2(q+1), , xn(tk+^), is for example described by a system of difference equations.

[0048] The system of equations further describes a law of evolution of the measurements made by the sensor. For example, the dynamic system of equations describing the evolution of the states of the object and the evolution of the measurements is of the form:

[0049] [Math 1] J4+1 = Axk + Bltk + where xk and xk+i represent the vectors respectively. yk+l = cxk+t+Vk+» comprising the n physical states at times 4 and 4+1- The vector 4 is equal to and is a vector of dimension n. The covariance matrix associated with the random vector 4 is for example designated by a matrix Qk. Each coordinate of the vector 4 is therefore a noise undergone by the corresponding coordinate of the vector xk+ï between times 4 and 4+1- The vector uk represents the control inputs applied to the system following the measurements carried out at time 4. In the example illustrated by [Fig.3], the vector M comprises two control inputs. It is of course possible that the control vector comprises a single control input, or a number of inputs greater than two. In the rest of the description, we will consider that the vector 11 is of dimension we N. The matrices A and B are matrices describing the evolution of the physical states of the system as a function of the physical states at the current time and the control inputs.In particular, matrix A is a transition matrix and matrix B is a control matrix linking control inputs to physical states. For example, matrices A and B are time-invariant, i.e., they do not depend on k. In other examples, at least one of the two matrices is time-dependent. For example, A, B can be time-dependent, i.e., A = Ak = A(4), B = Bk = B(tk), or any combination of constant and time-changing matrices. For example, A is a constant matrix and B = Bk is time-dependent. Matrix A is a square matrix of dimension nxn and matrix B is a matrix of dimension nx m. The vector 4 represents the measurements at time 4, and is for example of dimension P, p^ Ar *• In most applications, it is not possible to measure all states of the object. In other applications, the number of measurements P is less than the value of the integer n. In other applications, the measurements are indirect measurements of the state of the object. The vector A is a random vector of dimension P. In particular, the vector A models noises and disturbances occurring for example during the measurement by the sensor. For example, the vector A models the measurement errors. The covariance matrix associated with the random vector A is for example designated by a matrix Rk . The matrix C is an observation matrix of size P x ​​n. The matrix C is for example time-invariant. In other examples, the matrix C depends on time, that is to say on the value of k, that is to say C — Ck = C( 4 ) •

[0050] The measurement vectors 4 are for example stored, at each instant [Math.l] 4 , in volatile memory 112 or in non-volatile memory 110.

[0051] Applying a Kalman filter to this type of dynamic equation system makes it possible to estimate the states of the system from noisy measurements and a system state model.

[0052] [Fig.4] illustrates an example of the steps in a method for estimating the physical states of an object, for example a motorized object by applying a Kalman filter. More particularly, [Fig.4] illustrates steps of a numerical scheme implementing a Kalman filter.

[0053] By way of example, the digital scheme described in relation to [Fig.4] is implemented by the circuit 106 of the system 100 of [Fig.l], or by a circuit similar to the circuit 106 and connected to, or comprising, a sensor.

[0054] For example, in a step 400 (Initialization), initial estimates xqo and Pop are for example stored in a memory of the circuit. The estimate xqo represents an estimate of the state vector A at the initial instant. In other words, xqo is an estimate of the state vector at an initial instant % For example, in the case where the physical state values ​​of the object are known at instant % the estimate Xqo includes the real values ​​of the physical states. The estimate P^ is an estimate of a covariance matrix associated with the estimation error at the initial instant ?0- From an intuitive point of view, the matrix reflects the confidence that one has in the initial estimate ^qo.

[0055] A step 402 (Prediction) in the application of the filter corresponds to a prediction phase. In step 402, a prediction, or estimate, (Calculation of the predicted state) of the physical states and an estimate of the covariance matrix (Calculation of the covariance matrix) at the next instant are calculated. For example, step 402, for the moment, comprises the calculation, by a processor of the circuit, of an estimate .^0, for example by applying the formula — Aïqo + Biiq. The first realization of step 402 further comprises the calculation, for example by a processor of the circuit 106, of an estimate P\p. This calculation is carried out for example by applying the formula P AP^^A1 + Q ■ By way of example, the values ​​and Pjp are stored in a memory of the device. By way of example, the instructions allowing the execution of step 402 are stored in a non-volatile memory of the device.

[0056] Following the completion of step 402, the application of the Kalman filter continues in a correction step 404 (Correction / update). Step 404 corresponds to a phase of updating the predictions made in step 402. Step 404 comprises the calculation of a Kalman gain (Calculation of the Kalman gain), as well as the updating of the estimates of the physical states (Calculation of the estimated state) and of the covariance matrix (Update of the covariance matrix). For example, step 404 comprises, for the moment 4 the calculation, by the circuit processor, of the Kalman gain K1 by application, for example, of the formula jç _ p ç p çT + p y1. An update day is then obtained, for example by the circuit processor, by applying the formula + Kj^ym^me' an update of the co matrix variance P]| ] is calculated, for example by the processor of circuit 106, by applying for example the formula = ( j_Kic)P^(IK}C)T + K^K^- The estimates updates jq and P]|i are for example stored in a device memory.

[0057] Following the performance of step 404, the method resumes in a new performance of step 402. The new performance of step 402 is associated with a new instant U, successive to the instant considered in the performance of step 402 for the previous instant. The new performance of step 402 then allows a prediction, or estimation, of the physical states and of the covariance matrix on the basis of the estimations made in the previous step 404. A new performance of step 404 follows the new performance of step 402, and so on for each instant at which measurements are made.

[0058] Generally, at a time 4+1, a memory of the device comprises for example updated estimates x^ and associated with the time 4- The memory further comprises for example the matrices A, B. C, R^ and Qk as well as the control vector uk, if necessary. Step 402 then comprises the calculation, by the processor of the circuit, prediction, or estimation, of the physical states at the time 4+1, by application, for example, of the formula + Buk. Step 402 further comprises the estimation, by the processor of the circuit 106, of the covariance matrix at time h+ï by applying the formula Pk+^. — AP^{AT + Q ■ The estimated values ​​and are then, for example, stored in a memory of the device.

[0059] Following the performance of step 402 for the instant 4+i, the update step 404 is performed, for the instant 4+1- When performing step 404 for the instant 4+1, the Kalman gain associated with the instant 4+1 is calculated, for example by the processor of the circuit 106 and based on the estimates and matrices stored in the memory. The gain Kalman's coefficient is for example calculated on the basis of the formula K = Pk nX-7ÇCPk ^CT + R. I'L estimation of physical states at time 4 is then updated by applying the formula + Kk Finally, the estimate of the covariance matrix at time 4+1 is updated, for example by applying the formula n+w = (i-Kk+1c)pk+^(i^^

[0060] However, the application of this numerical scheme, as is, risks not being numerically stable. Rounding errors occur, for example, and are cascaded at each step of the scheme; the estimates made are then erroneous. Moreover, mathematically, for each instant 4, the covariance matrix is, theoretically, a positive definite symmetric matrix. The implementation of the numerical scheme described in relation to [Fig.4] does not allow this theoretical property to be numerically guaranteed.

[0061] [Fig.5] is a flowchart illustrating steps for implementing a method for estimating physical states of an object, such as for example a motorized object or any object whose physical states are described by equations of a Kalman filter, according to an embodiment of the present description. In particular, the estimation method described in relation to [Fig.5] allows the application of a Kalman filter by means of a stable and fixed-point numerical scheme. Indeed, in the implementation of the numerical scheme described in relation to [Fig.5], the covariance matrices of the Kalman filter remain symmetrical and positive definite. Furthermore, the Kalman filter constructed and implemented according to the scheme of [Fig.5] applies to a fully normalized state model, at the level of physical states, measurements and matrices.

[0062] In a step 500 (State model (physical state variables)), parameters, such as for example the matrices A, B and C, are stored in a non-volatile memory and / or in a volatile memory of another processor.

[0063] As an example, the matrices A, B, and C are obtained by linearization of nonlinear state equations which leads to the implementation of an extended Kalman filter.

[0064] For example, initial data values ​​are stored in the non-volatile memory and / or in the volatile memory of the other processor. The initial data values ​​include, for example, the physical states at a time ^o, and / or a measurement of at least one of the physical states at time / q and / or an estimate of the associated covariance matrix Pop and / or an initial control vector zzo.

[0065] In a step 501 (Normalization of the physical quantities), the physical state vectors xk, the measurement vectors 3;, and the control vectors llk are normalized for each instant / jt-The choice of this normalization is for example carried out on the other processor, upstream of the programming of the Kalman filter on the circuit 106. The person skilled in the art knows the evolution over time of the system of dynamic equations, or state model, describing the motorized object. For example, the person skilled in the art knows how to calculate, for each instant for example by using a computer, an interval in which the values ​​of the vector xk are located, as well as intervals in which the values ​​of the vector xk are located respectively. values ​​of the vector and the vector

[0066] By way of example, step 501 comprises the search, for example on another computer and upstream of the programming of the Kalman filter on the circuit 106, for transformation matrices TB and Tc. The transformation matrices TA, TB and Tc are such that the vectors ÿk and ük, where xk = TAxk, y — TBy and ük — Tcuk are bounded, for each instant tk- For example, the transformation matrices TA, TBet Tc are such that each of the components of each of the vectors X^ T and ük belongs to the interval [ - 1; 1], or more generally to an interval of the form [a è], where a and b are two real numbers, where a < b.

[0067] In another example, the search for the transformation matrices TA, TB and Tc is carried out by computer, and the transformation matrices TA, TB and Tc are for example then stored in a non-volatile memory and / or in a volatile memory of another computer, upstream of the programming of the Kalman filter on the circuit 106.

[0068] The search for the transformation matrices TA, TB and Tc is carried out for example by means of the execution of optimization algorithms. The methods for searching for such matrices are known and are within the reach of the person skilled in the art.

[0069] In some cases, the vectors xk and / or and / or uk are already bounded. In this case, the transformation matrix TA, and / or TB and / or Tc is equal to the identity matrix. When, for any instant 4, the three vectors xk, and have their components bounded, step 501 is, for example, omitted.

[0070] Following the completion of step 501, the evolution of the normalized vectors Xk and ÿ'k is then described by a new system of dynamic equations, or state model, normalized 502 (Normalized State Model). The system of dynamic equations, or state model, 502 is then of the following form: [Math 2] [ ^t+1= + TABry,k + [ yk = Tccr'xk + Tcvt

[0071] Following the completion of step 501, the method continues in a step 503 (Normalization of the state model matrices). For example, step 503 is performed on another computer, upstream of the programming of the Kalman filter on the circuit 106. Step 503 comprises the search for two other transformation matrices VT] and W2 normalizing the matrices TaATa, TaBTb and T AC. Indeed, step 501 is not sufficient to guarantee the normalization of the matrices TaATa, TaBTb and TAC. The matrices VEj and W2 sought are such that each coefficient of each of the matrix products W (TaBT~b and W2TcCT\ is bounded, for example, in absolute value by a real M, for example equal to 1.

[0072] The search for the transformation matrices and W2 is carried out by another or diner, upstream of the programming of the Kalman filter on the circuit 106. According to one embodiment, the matrices W, and W2 sought are matrices diagonals, that is to say matrices whose non-diagonal elements are all null.

[0073] The transformation matrices and W2 are, for example, then stored in a non-volatile memory and / or in a volatile memory of the computer used for their calculation, upstream of the programming of the Kalman filter on the circuit 106.

[0074] The search for the transformation matrices VF3 and W2 is carried out for example by means of the execution of optimization algorithms. The methods for searching for such matrices are known and within the reach of the person skilled in the art.

[0075] Following the completion of step 503, the evolution of the normalized vectors and V*, where ÿk = W is then described by a new system of dynamic equations, or state model, 504 (Fully normalized state model (states and matrices)). The dynamic state model 504 is then a fully standardized system. More particular Basically, the fully normalized system of equations 504 is of the following form: [Math 3] Wà, - ÀXlr + Biu + h, 1 A+l A ' K ÿk=c^+vk where the matrices A, B and C, respectively equal to At = W\TaATa' KB = W^T^Tb and C = W2TcC7^A, are normalized. The vector and Vk are respectively equal to îî?. — Wet at vk = VV27'cvÂ.. The vector models noise and / or perturbations of the states of the fully normalized system of equations 504 at time The vector Vk models the noise in the measurements, including for example measurement errors, of the system of equations, or state model, fully normalized 504 at time tk.

[0076] The fully normalized state model 504 then has the structure of a linear descriptor system.

[0077] In another embodiment, the linear descriptor state model is obtained after a step of linearizing a non-linear state model.

[0078] Following the completion of step 503, the method continues in a step 505 (Expression of the Kalman filter for descriptor system). For example, step 505 is performed upstream of the programming of the Kalman filter on the circuit 106. In step 505, a descriptor Kalman filter 506 (Kalman filter for fully normalized descriptor state model) for the system of dynamic equations, or state model 504 is constructed.

[0079] According to one embodiment, the Kalman filter 506 is a Kalman filter for the linear descriptor dynamic systems. Such a filter is described in more detail by R. Nikoukhak, A. S. Willsky, and B. C. Levy in the article "Kalman filtering and Riccati equations for descriptor systems" published in 1992 in IEEE Transactions on Automatic Control.

[0080] For example, for each instant 4+1, the noise covariance matrix P^+^+] of a Kalman filter for a descriptor dynamic linear system described by a fully normalized descriptor state model 504 follows the following recursive relationship: [Math 4] where the central matrix is a block matrix, each block being a matrix.

[0081] An estimation Pæ" application of a Kalman filter for a system re presented by a fully normalized descriptor linear dynamic model, is then obtained by: [Math 5] ^k+>+1 = Pk+ty+lW[( + Qk ) ( + Bük ) + Pk+ïk+lC Pk ÿk+i-

[0082] The expressions of the covariance matrix Pk+^k+i and the estimate .X^^+j of the normalized states for the fully normalized descriptor state model thus defined constitute the construction of the Kalman filter for the linear descriptor system for the system of dynamic equations 504.

[0083] However, the mathematical properties of the covariance matrix Pk^+1 such as being symmetric and positive definite, may not be verified numerically. An algorithm implementing a Kalman filter for a dynamic system represented by a descriptor linear state model as previously described, is then not systematically numerically stable.

[0084] Step 505 is for example carried out upstream of the programming of the Kalman filter on the circuit 106, by another computer. When the stored instructions are executed by the processor 108, these for example call upon the matrices Wf, A, B, C and Rk, for example stored in the non-volatile memory 110 and / or in the volatile memory 112. When they are executed by the processor 108, the instructions for example also call upon the normalized vectors, Ük and In addition, when they are executed by the processor 108, the instructions call upon covariance matrices P^ whose theoretical mathematical properties, including symmetry and being positive definite, are preferably verified at each instant 4.

[0085] To ensure that the covariance matrices P^ remain symmetrical and positive definite, they are obtained via a factored expression of the Kalman filter for a descriptor dynamic system described by a descriptor linear state model, carried out in a step 507 (Factorization of the Kalman filter for a fully normalized descriptor state model) carried out on the circuit 106.

[0086] According to one embodiment, during step 507, the covariance matrices of the Kalman filter for the fully normalized descriptor linear dynamic system are factorized, for each instant 4- The factorization of the covariance matrices makes it possible to guarantee that they are, for each instant 4, symmetric and positive definite. The digital implementation of the Kalman filter for the fully normalized descriptor state model 506 in factored form makes it possible to guarantee that the filter is numerically stable with positive definite covariance matrices P. In addition, the digital implementation of this filter can be carried out with fixed-point arithmetic on the processor 106 given the normalization steps carried out upstream of the application of the Kalman filter, on another processor. For example, the factorization of the covariance matrices is carried out on the fly, at each instant 4-

[0087] The covariance matrices Pk+^+i and the estimates ,X / .-+^+î follow principally matrix palement: [Math 6] y1 However for each instant 4, this matrix is regular but indefinite. Using the Schur complement, the matrix - which by definition is negative definite, appears in the explication of the matrix La matrix Mk being undefined, the Cholesky decomposition, allowing its decomposition into a product UkUk, where Uk is an upper triangular block matrix, is not directly applicable.

[0088] However, a non-real upper triangular matrix, i.e. with complex coefficients, U t exists because the matrix Mk is symmetric. Therefore, we note: [Math 7] [V with: c =u^uk, 0 1 [Math 8] 3Jc U^' step 507 includes searching for matrices

[0089]

[0090]

[0091]

[0092]

[0093]

[0094]

[0095] In fact, following the development of the UkUk product, we obtain that: [Math 9] (¾)7 = Uu = 0, (¾ fu4k=wt ( «u ) TUu + (¾) Tuik=M uk )Tu5k=cT, Uu+iU^k) U 5k + (U «:) ^151=0- The matrices Q and Pty being by definition positive definite, the decomposition, or the Cholesky factorization applies. Therefore, there exists an upper triangular matrix jjQ such that q _ ( yQ j T yQ and an upper triangular matrix such that p>*= (yi)Tu¥k- Thus, by applying a QR decomposition, we obtain that [Math 10] UjÀ °ù 1 is an orthogonal matrix. egg The application of QR decomposition is carried out, for example, by executing instructions implementing a QR decomposition algorithm implemented by a processor, or by a computer. Furthermore, following the product expansion we deduce that the matrix is ​​a zero matrix. We then deduce that (y \TU —R P31 oomequent that the matrix U is the Cholesky factor of the matrix Rk, which is, by definition, positive definite. The expression of the matrix U4j< is obtained by solving a system of tri equations angular formed by the matrices Similarly, the expression of the U5k matrix is ​​obtained by solving a system triangular equation formed by matrices (¾)76^7 For example, systems of triangular equations are solved by substitution. For example, the expressions of the matrices U and U5I( are for example obtained following the execution of a substitution algorithm by the processor 108. The matrices U and U5;.: are for example then stored in the non-volatile memory 110 and / or in volatile memory 112.

[0096] The explicit expression of is then obtained from the expressions of and and by considering a matrix Vk, such that EZi — iU6k where i is the imaginary unit, such that j2 — - 1. Thus, the product _ U^kU6k is such that - = VkVk = U^U^+U^U^- A QR decomposition then applies and allows to obtain the relation: [Math 11] [L1 pRA LO J where r6 is an orthogonal matrix.

[0097] The application of the QR decomposition is carried out for example by the execution of instructions implementing a QR decomposition algorithm implemented by the processor 108. In particular, following the execution of the instructions implementing the QR decomposition algorithm, the value of the matrix Vk is obtained. The value of the matrix Vk obtained is for example stored in the non-volatile memory 110 and / or in the volatile memory 112.

[0098] Returning to the expression of the covariance matrix for the filter of Kalman for fully normalized descriptor state model 506, in other words the expression: [Math 12] ' 0 we obtain that the matrix / >^ = -[0 oz]m à ' o = -[oo ijufu? o , LzJ Lz. of covariance Pk+^c+ï is such that Pk+^+i = - VkV'k- Following its calculation, for example by the processor 108, the matrix is ​​for example stored in the non-volatile memory 110 and / or in the volatile memory 108. The matrices Pk+^+\ thus obtained for each instant 4+1 are all symmetric and positive definite. The mathematical properties of the covariance matrices Pk+^+] are then numerically guaranteed. It is also guaranteed that the numerical scheme implementing the application of the Kalman filter for fully normalized descriptor state model 506 on the basis of the matrices thus obtained is numerically stable.

[0099] For example, only the matrices Vk are stored in the non-volatile memory 110 and / or the volatile memory 112, the matrices Ujkj i — ] ■ ■ ■ ? 6, only intervening in the calculation of Vk and not directly in the expression of the factored Kalman filter for fully normalized descriptor state model at time 4+2-

[0100] Following the factorization step 507, a factorized Kalman filter for a system modeled by a fully normalized, descriptor state model 508 (Filter of Kalman factored for fully normalized descriptor state model) is obtained. This filter is initialized by a matrix which is the Cholesky factor of the initial covariance matrix p^ — the latter being by construction symmetric and positive definite, by an initial vector The factorizations Vk of the covariance matrices P^+]^+] are obtained according to the expressions established in step 507, on the processor 108 of the circuit 106.

[0101] The updated estimates of the states of the fully normalized descriptor state model are then calculated, for example by the processor 108 and on the basis of the matrices W b jp and the control and measurement vectors .L + i , and covariance matrices p. , — yf y previously stored in the non-volatile memory 110 and / or in the volatile memory 112. In particular, at each instant ^+1, the updated estimate ^+1|^+1 of the states of the fully normalized descriptor state model is for example obtained by the processor 108 on the basis of the expression: [Math 13] ■^+ l+! = A-+g-+i^r(+ Q k ) ( Ai,® + ) + P^CR k y k+k

[0102] Steps 500 to 507 are performed, for example, on another computer, and upstream of the programming of the Kalman filter on the circuit 106. Starting from the physical state model of step 500, steps 501, 503 and 505 which lead respectively to the models of steps 502, 504 and to the expression of the descriptor Kalman filter of step 506 are then qualified as steps performed offline unlike step 508 which is performed by the processor 108 upon receipt of an instruction for estimating the states of the fully normalized descriptor state model, on the basis of the factorized Kalman filter for fully normalized descriptor state model 508.

[0103] In another configuration, the normalized estimated state is expressed from the factorization of the matrix Mk = Uk Uk- The updated estimate x£+]^+i of the states of the fully normalized descriptor state model is then obtained by the processor 108 based on the expression: [Math 14] - [ 0 0 / ] ( UlUk ) + Bük h+i 0

[0104] According to one embodiment, the resolution of the above equation is carried out on the processor 108 of the circuit 106 using a propagation technique, in two steps, known to the person skilled in the art, applied to the block matrices i — 1, ... 6, established during the determination of the matrix P_(k+llk+l), and stored on the non-volatile memory 110 or the volatile memory 112 of the circuit 106.

[0105] In particular, a first step of the propagation technique is to solve the following matrix equation: [Math 15] H v2, V3. :_1 z^ Or The elements of the vector v are then such that: [Math 17] V1 = + 1'2 -

[0106] A second step of the propagation technique consists of determining the elements of a vector such that: [Math 18] U» 0 y Therefore, we obtain that -3 - xk+ 1|A+1 “

[0107] [Fig.6] is a block diagram illustrating an example of a system 600 comprising a geared motor 601.

[0108] The geared motor 601 is for example an actuator motor of a windshield wiper blade. By way of example, the geared motor 601 comprises an electric motor, a transmission and a battery. In other examples, the geared motor 601 comprises a heat engine and a fuel reserve. The vector of the physical states of the geared motor comprises for example the position, the speed and the acceleration of the geared motor 601. In another example, the vector of the physical states of the geared motor comprises for example the position determined by an angle, the angular speed and the angular acceleration of the geared motor 601.

[0109] The system 600 further comprises, for example, a sensor 602 (Sensor), for example connected to the geared motor 601. By way of example, the sensor 602 is only capable of measuring one or more components of the position of the mo- toreducer 601. For example, the sensor 602 is configured to perform measurements at regular time intervals, for example of the order of a few milliseconds. The times at which the sensor 602 performs measurements are for example denoted by times ke N.

[0110] The evolution of the vector of physical states and measurements is for example described by the dynamic model of the form: [Math 19] X^+1 — Axk + Buk + as described previously. The matrices A, B and C are yk=Cxk+vk, for example matrices adapted to the geared motor 601. The covariance matrices Qk and Rk associated with the vectors and are also adapted to the geared motor 601 and the sensor 602. The matrices A, B and C and the covariance matrices Qk and Rk are therefore parameters specific to the geared motor and the sensor in question. The determination of these parameters is known and is within the reach of the person skilled in the art.

[0111] The sensor 602 is further configured to transmit the measurements made at each instant 4 to a processor 604 (CPU). The processor 604 comprises for example an internal memory comprising instructions 606 (Kalman filter) allowing the application of a factored Kalman filter and for the fully normalized descriptive system. In particular, the dynamic model describing the evolution of the physical states and the measurements has been transformed, according to the embodiment described in relation to FIG. 5, and more particularly according to steps 501 and 503, into a fully normalized descriptive model. By way of example, this transformation is carried out upstream, for example on another device such as for example another computer. The modified parameters, i.e. the matrices A, B, C, Q.and are then calculated via this other device and are then stored in a memory (not shown) connected to the processor 604, or forming part of the processor 604.

[0112] The processor 604 is further configured to generate, at each instant ^+1, the vector ÿk+ î on the basis of the measurement vector transmitted by the sensor 602 and according to the method described in relation to [Fig.5].

[0113] The processor 604 further comprises a control circuit 608 (Control circuit) configured to generate, at each instant, a control vector uk (u). The processor 604 is further configured to transform the control vector 11 k into a vector l <k selon le procédé décrit en relation avec la figure 5. le vecteur est alors stocké dans mémoire interne du processeur 604.

[0114] The control circuit 608 is further configured to transmit the command vector uk to the geared motor 601. The actuator of the geared motor 601 is then configured to apply the received command to the motor.

[0115] For example, the vectors ÿk} and Ük are stored in a volatile manner and are for example deleted upon receipt, by the processor 604, of the vectors and Ùk+\-

[0116] The internal memory of the processor 604 stores for example in addition the factorizations of the covariance matrices generated, by the processor 604, according to the application of the factorization step 507 described in relation to [Fig.5] and according to the instructions 606.

[0117] Instructions 606 further allow the generation of the updated estimate À7+]^+i (Estimated States). The vector then comprises estimates of the normalized states of the fully normalized descriptor state model of the geared motor 601 at time ^+i. In particular, instructions 606 allow the application of a digital scheme of a descriptive Kalman filter, fully normalized, and factored as described in relation to [Fig.5]. In particular, instructions 606 allow the implementation of a stable, fixed-point digital scheme.

[0118] The processor 604 is further configured to transmit the estimated vector to the control circuit 608. The control circuit 608 then generates, on the basis of the vector Xfc+1^+Î, and under the execution of instructions implementing a control algorithm, a new control vector ük+ï, and subsequently a vector

[0119] The instructions implementing the control algorithm are specific to the system 600 and their programming is known and within the reach of those skilled in the art.

[0120] An advantage of the described embodiments is that the numerical scheme resulting from the application of the Kalman filter is numerically stable. In particular, the mathematical properties of the covariance matrices are numerically guaranteed. Furthermore, the calculations performed are fixed-point.

[0121] Various embodiments and variants have been described. The person skilled in the art will understand that certain features of these various embodiments and variants could be combined, and other variants will appear to the person skilled in the art. In particular, the search for the transformation matrices, described in relation to steps 501 and 503, can be carried out in various ways. Generally speaking, the search for such matrices is known and within the capabilities of the person skilled in the art. In addition, the different parameters, such as matrices A, B and C, of ​​the system of dynamic equations may depend on time. The person skilled in the art will then know how to adapt the normalization steps to this case.Furthermore, matrices A, B and C can be obtained by linearization of a non-linear state model, leading to an extended Kalman filter to which steps 501 and 503 are applied which lead to a fully normalized descriptor state model, which makes it possible to implement the numerical scheme of a factorized Kalman filter for a fully normalized descriptor state model. standardized.

[0122] Normalization steps 501 and 503 may be omitted. Step 507 is then applied to a descriptor state model 500, leading to a numerically stable numerical scheme for the Kalman filter for a descriptor state model that is not necessarily normalized.

[0123] Finally, the practical implementation of the described embodiments and variants is within the reach of the person skilled in the art from the functional indications given above. In particular, the objects as well as the technical fields to which the described embodiments apply are not limited.< / k>

Claims

1. Claims Method for estimating, by a processor, a vector x*+i comprising values ​​of physical states of a motorized device at an instant tk+^ k, the physical states comprising at least one indication of the position of the device, the evolution in discrete time of the states of the device being described by a system of recursive dynamic equations of the form ( X^j — Axk + Buk + , where the variable u represents a (yk=cxk+vk control vector generated by a control circuit of the motorized device, the variable H is a random vector, modeling noise and disturbances external to the device, having a Q matrix as covariance matrix; the variable y comprises measurements, carried out by a sensor, of at least one of the physical states and / or the indirect measurement of at least one of the physical states; the variable v is a random vector, modeling noise and measurement errors, having a R matrix as covariance matrix; and the matrices A^ B and C are respectively matrices of evolution of the physical states, the control inputs and the measurements, the method comprising: - the normalization of the system of dynamic equations, the normalization comprising the search, by the processor, for a matrix W! and a matrix W2 such that the matrices A = WjA, B ​​— and C = W2C are bounded, and their storage in a memory; - the factorization of a descriptor Kalman filter having, for the moment tk, a covariance matrix Pkk. the factorization comprising the calculation of a triangular matrix Uk per block such that -T AP^A +QS jy ", where is the co- matrix c variance associated with and where Rk is the covariance matrix associated with Wï, and the storage in memory of at least one of the matrices composing the block matrix Uk; and - the calculation, by the processor connected to the sensor and on the basis of the vector comprising the measurements made by the sensor, of an estimate of the vector x*+i on the basis of the factored descriptor Kalman filter. Method according to claim 1, in which the normalization of the

3.

4.

5.

6.

7.

8.

9. system of dynamic equations further comprises, before the search for the matrices Ws and W2, the search, by the processor, for matrices TA, TBetTc such that, for each instant the variables xk = TAxk, yk=T Byk andük = T cuk are bounded and in which the search for the matrices and W2 is carried out so that the matrices À = WB = W}TABrB and C = W2TcCTi are bounded. The method of claim 2, wherein the covariance matrix P^ is guaranteed to be numerically symmetric and positive definite and wherein calculating the estimate xA.+)£+i comprises performing, by the processor, instructions stored in memory, instructions programming the calculation of Xk+^+i-Pk+^+iW^ÀP^À +Qk) ÇÀXj^ + Bük ) +Pk+^+ï^rRk , where ÿ} = i and where depends on the matrix Uk. The method of claim 3, wherein the matrix Pk+ ^+] is such that Pk+^^ = V^V^' °where ^k is a matrix such that jl+i IU^', where r6 is an orthogonal matrix and where and Vik, are blocks of the triangular matrix Uk. The method of claim 4, wherein the matrix is ​​such that ( u =^ / 1111 ma,rice vuest ,eiie ( uit) tu =e-where the matrix U ; is equal to Àp , 2 T + Q and the matrix is ​​the Cholesky factor of the matrix Rk. A method according to any one of claims 1 to 5, wherein the matrices Wj and W2 are diagonal matrices. A method according to any one of claims 1 to 6, wherein at least one of the matrices A, B or C is time dependent. Method according to any one of claims 1 to 7, in which at least one of the matrices A, B and / or C is obtained by non-linear state model linearization. Method according to any one of claims 1 to 8, in which the search, by a processor, of at least one of the matrices and W2 is carried out by applying an optimization algorithm. A method according to any one of claims 1 to 9, wherein the estimate x^+1^+1 is a vector of size ”, n being an integer, and the vector yk+i is a vector of size m, m being an integer less than or equal to n.

11. System comprising: - a motorized device, the evolution of one or more physical states of which is described by a system of recursive dynamic equations of the form ( Xk+ f = Axk + Buk 4- , the variables and x*+i being | yk=Cxk+vk respectively vectors describing one or more physical states of the motorized device at a time tk and a successive time tk+], the variable uk representing a control vector generated, for the time tk, by a control circuit of the system, the variable being a random vector, modeling noise and disturbances external to the device for the time tk, having as covariance matrix a matrix Qk; the variable Vk comprising measurements, carried out by a sensor, of at least one of the physical states and / or at least one indirect measurement of one of the physical states, at the time tk; the variable vk is a random vector, modeling noise and measurement errors for the time tk, having as covariance matrix a matrix Rk; - a memory in which matrices A, B and C are stored, matrices and W2, such that the matrices A = W}A, B = W,B and c = W2C are bounded, and for storing sets of matrices and Rk, where for each instant tk, ke N, is the covariance matrix associated with Wet where Rk is the covariance matrix associated with W2vk, the memory further storing, in association at each instant tk a covariance matrix Pÿ^ such that uTkuk^ block ; - the control circuit configured to generate the control data uk for each instant tk; - the sensor configured to generate, at each instant tk, the vector yk comprising the measurement of at least one of the physical states and / or the indirect measurement of at least one of the physical states of the motorized device, of the motorized device at the instant tk, the sensor furthermore being , a triangular Uk matrix by 'k 0 Rk configured to transmit the generated vector to a processor; - the processor configured for: generate a vector ÿk based on the received vector and the matrix W2 generate a vector ük based on the control data generated by the control circuit and the matrix W!; generate an estimate xk+i\k+l of the physical states of the motorized device at time ^+i based on the matrices A, B, C, i, ^2, Qk> Rk and order the storage, in the memory of the covariance matrix Pk+^+ï of the estimate ^+^+r

12. The system of claim 11, wherein the control circuit is configured to generate control data based on the estimate

13. The system of claim 11 or 12, wherein the memory further stores matrices TA, TB. Tc such that, for each instant tk, the variables xk = T Axk, ÿ = TBy and = Tcuk are bounded and in which the matrices and W2 are such that the matrices At = W {TaAT-1, B = W {TaBT~b and c = W2TcC7^ are bounded, the processor being further configured to: - generate the vector ÿk based on the matrix Tc; and - generate the vector ük based on the matrix TB.

Images