Kalman filter for estimating the states of a motorized device

Abstract

Kalman Filter for Estimating the States of a Motorized Device. This description concerns a method for estimating, by a processor, a vector comprising the values ​​of the physical states of a motorized device at a given time. The physical states include at least one indication of the device's position. The discrete-time evolution of the device's states is described by a system of recursive dynamic equations. The method comprises: - normalization of the system of dynamic equations; - factorization of a descriptor Kalman filter; and - calculation, by the processor connected to the sensor and based on a vector comprising the measurements taken by the sensor, of an estimate of the vector based on 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] This description relates generally to methods and devices for estimating the physical states, in particular the position and velocity, of a motorized device, by applying a Kalman filter. Previous technique

[0002] Kalman filters are used in a wide range of technological fields. In particular, Kalman filters make it possible to estimate the states of a dynamic system whose measurements are incomplete and / or noisy. For example, applying a Kalman filter makes it possible to estimate the position and / or velocity and / or acceleration of a motorized object, such as 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 performs estimations using a Kalman filter, errors, such as rounding errors, accumulate and propagate during the estimation steps. The estimations are then distorted and / or the associated covariance matrices lose their theoretical property of definite positivity. Commands generated based on these estimations for controlling the motorized object are also distorted, leading to undesired behavior of the object, such as, for example, excessive power consumption or hardware damage.

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

[0005] One embodiment provides a factored Kalman filter for a system described by a state-descriptor model. In particular, the state-descriptor 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 device's position, the discrete-time evolution of the device's states 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 Q; the variable y comprises measurements, performed 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 R; and the matrices A, B, and C are respectively evolution matrices of the physical states, control inputs, and measurements, the process 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 Kalman filter descriptor having, for the moment, a covariance matrix P^, the factorization including the calculation of a block triangular matrix Uk such that , where is the covariance matrix associated with and where Rk is the covariance matrix associated with 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 yk comprising the measurements made 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 includes, 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 wherein the calculation of the estimate x^^ comprises the execution, by the processor, of instructions stored in 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 Usont 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 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 time tk.

[0013] According to one embodiment, at least one of the matrices A, B and / or C is obtained by linearization of a non-linear 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, where the variables xk and x*+ i are vectors yk=Cxk+vk respectively 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 its covariance matrix a matrix Qk; the variable yk comprising measurements, made by a sensor, of at least one of the physical states and / or at least an 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 its covariance matrix a matrix Rk; - a memory in which matrices A, B, and C, matrices W and W2, are stored such that matrices A → WjA, B ​​= W^B, and C = W2C are bounded, and for storing 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 storing further, 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 uk control data 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 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; generate 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 / ^ command the storage, in 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+\ on the basis of the estimated 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 on the basis of the matrix Te; and - generate the vector ük on the basis of the matrix TB. Brief description of the drawings

[0019] These features and advantages, as well as others, will be described in detail in the following description of particular embodiments, given by way of non-limiting example, in relation to the accompanying figures, among which:

[0020] [Fig.1] is a block diagram representing 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 taken 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 process for estimating the physical states of a motorized object by applying a Kalman filter;

[0025] [Fig. 5] is a flowchart illustrating the steps in implementing the process of [Fig. 4] according to one 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 description. Description of the implementation methods

[0027] The same elements have been designated by the same reference numerals in the different figures. In particular, the structural and / or functional elements common to the different embodiments may have the same reference numerals 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 shown 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 together, this means directly connected without intermediate elements other than conductors, and when referring to two elements coupled together, this means that these two elements can be connected or linked through 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", "superior", "inferior", 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 "approximately", "roughly", and "in the order of" mean within 10%, preferably within 5%.

[0032] Figure 1 is a block diagram representing an example of a system 100 configured for estimating one or more physical states of a motorized object 102 (DEV). By way of example, the object 102 is an airplane, a satellite, a motorized component 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 velocity, and / or its acceleration. The physical states of the object are, for example, described by a vector of dimension fl, where n is 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 three-dimensional space, n is equal to 9. In another example, the object is a geared motor or actuator, belonging, for example, to a larger device such as an automobile. The geared motor is, for example, a windshield wiper motor. In this example, the object's states are, for example, an angle, an angular velocity, and / or an angular acceleration.

[0033] The object's states 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 integrated into the device comprising the object 102. For example, the sensor(s) are configured to measure an angle or a position in 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 includes a processor 108 (CPU), such as a central processing unit or a microcontroller, configured to calculate the states of the object 102 based on measurements taken by the sensor(s) 104 and transmitted to the circuit 106. By way of example, the circuit 106 further includes 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 (RAM) configured to receive and store the measurements transmitted by the sensor(s) 104.

[0035] The system further includes a control circuit 116 (CTRL) configured to transmit control input data to the object 102. For example, the control circuit 116 is configured to generate the control input data based on estimates of the object's physical states, 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 includes, for example, an antenna (not shown) configured to transmit these RF signals, and the control circuit 116 includes, for example, an antenna (not shown) configured to receive these RF signals.In another example, control circuit 116 and circuit 106 are integrated into the same device.

[0036] By way of example, the control data is transmitted from the control circuit 116 to the object 102 via a wired connection. 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 includes, for example, an antenna (not shown) configured to transmit these RF signals, and the object 102 includes, for example, an antenna (not shown) configured to receive these RF signals.

[0037] Figure 2A illustrates an example of a motorized object. In the example illustrated by Figure 2A, the object takes the form of an airplane. 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 in [Fig. 2A], three successive positions of the aircraft 200 are shown. By way of example, the three aircraft positions are positions measured by the sensor 202 at regular time intervals. By way of example, the time interval is a fixed value and is on 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 shown) configured to estimate, in addition to the next position of the aircraft, for example its speed and 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 taken by sensor 202 on aircraft 200.

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

[0043] According to one embodiment, state estimates, such as, for example, the position, velocity, and / or acceleration of an object, are obtained by applying a Kalman filter to the measurements taken by the sensor(s). Applying a Kalman filter reduces errors due to noise, disturbances, and measurement errors in the estimation of one or more states, such as the position, velocity, and / or acceleration of an object, and allows for the estimation of observable but unmeasured states.

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

[0045] By way of example, at a given instant the physical states of the object are described by a number e N * of variables x^t^, x2(tk), , Xn(tk) (State variables at time k). For example, n = 9 and the variables xt(tk), x2(tk), and x3(tk) are the coordinates describing the object's position in three-dimensional space. Similarly, the variables x^(tk), x^(tk), and x6(tk) describe, for example, the The linear velocity of the object in three-dimensional space, and the variables X7(tk), x8(), and 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. For example, the space in which the object moves can vary and be one-dimensional or two-dimensional. Similarly, instead of describing position, velocity, and acceleration, the states can describe other types of position data, such as angle, angular velocity, and / or angular acceleration, etc. Likewise, the state variables can describe other physical quantities, such as the torque applied to a drive shaft of an electric motor, the current and / or voltage applied to the terminals of an electric motor, etc.

[0046] For example, at time tk, command data (tk) and (tk) (Control inputs) are applied to the object. For example, control inputs can be used to control a change of direction, a deceleration, or an acceleration 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 yet another example, more than two control inputs are applied to the object.

[0047] By way of example, the evolution of the object's physical states is further 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 n, that is, the same dimension as the number of physical state variables. The evolution of the state variables, also called the physical "states" of the object, that is, the states at time 4+i which are denoted jq(q+1), x2(q+1), xn(tk+1), is, for example, described by a system of difference equations.

[0048] The system of equations further describes a law governing the evolution of the measurements performed by the sensor. By way of example, the dynamic system of equations describing the evolution of the object's states 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 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, denoted by a matrix Qk. Each coordinate of the vector 4 is therefore a noise experienced 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 performed at time 4. In the example illustrated by [Fig. 3], the vector M comprises two control inputs. It is, of course, possible for the control vector to comprise only one control input, or a number of inputs greater than two. In the remainder 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, meaning they do not depend on k. In other examples, at least one of the two matrices is time-dependent. For example, A and B can be time-dependent, meaning 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 n x n, and matrix B is a matrix of dimension n x 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 the object's states. 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 object's state. The vector A is a random vector of dimension P. In particular, the vector A models noise and disturbances that occur, for example, during measurement by the sensor. As an example, the vector A models measurement errors. The covariance matrix associated with the random vector A is, for example, denoted 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, on the value of k, i.e., 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 dynamical equation system allows the system states to be estimated from noisy measurements and a system state model.

[0052] Figure 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 specifically, Figure 4 illustrates steps in 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.1], or by a circuit similar to the circuit 106 and connected to, or comprising, a sensor.

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

[0055] A step 402 (Prediction) in the application of the filter corresponds to a prediction phase. In step 402, a prediction, or estimation (Calculation of the predicted state) of the physical states and an estimation of the covariance matrix (Calculation of the covariance matrix) at the next instant are calculated. By way of example, step 402 currently includes 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 the step Step 402 further includes the calculation, for example by a processor in circuit 106, of an estimate P\p. This calculation is performed, for example, by applying the formula P = AP^^A1 + Q. As an example, the values ​​P and P are stored in a memory of the device. As an example, the instructions enabling 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 includes the calculation of a Kalman gain (Calculation of the Kalman gain), as well as updating the estimates of the physical states (Calculation of the estimated state) and the covariance matrix (Update of the covariance matrix). By way of example, step 404 includes, For now, 4. The calculation, by the circuit's processor, of the Kalman gain K1 by applying, for example, the formula jç _ p ç p çT + p y1. A setting to The day is then obtained, for example by the circuit's 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 memory of the device.

[0057] Following the execution of step 404, the process resumes in a new embodiment of step 402. This new embodiment of step 402 is associated with a new time U, successive to the time considered in the embodiment of step 402 for the previous time. The new embodiment of step 402 then allows for a prediction, or estimation, of the physical states and the covariance matrix based on the estimations performed in the preceding step 404. A new embodiment of step 404 follows the new embodiment of step 402, and so on for each time at which measurements are taken.

[0058] Generally, at time 4+1, a memory of the device includes, for example, updated estimates x^ associated with time 4. The memory also includes, for example, the matrices A, B, C, R^, and Qk, as well as the control vector uk, if necessary. Step 402 then includes the calculation, by the circuit processor, of the prediction, or estimation, of the physical states at time 4+1, by applying, for example, the formula + Buk. Step 402 also includes the estimation, by the processor of 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 execution of step 402 for time 4+i, the update step 404 is executed, for time 4+1. During the execution of step 404 for time 4+1, the Kalman gain associated with time 4+1 is calculated, for example by the processor of the circuit 106 and based on the estimates and matrices stored in memory. The gain Kalman, for example, is calculated based on the formula K = Pk nX-7ÇCPk ^CT + R. 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, may not be numerically stable. Rounding errors occur, for example, and are cascaded at each step of the scheme, resulting in erroneous estimates. Moreover, mathematically, for each time step 4, the covariance matrix is, theoretically, a symmetric positive-definite matrix. The implementation of the numerical scheme described in relation to [Fig. 4] does not guarantee this theoretical property numerically.

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

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

[0063] By way of 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] By way of 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 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 step 501 (Normalization of 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, made on the other processor, upstream of the programming of the Kalman filter on the circuit 106. A person skilled in the art is familiar with the evolution over time of the system of dynamic equations, or state model, describing the motorized object. For example, a person skilled in the art knows how to calculate, for each instant, for example using a computer, an interval in which the values ​​of the vector xk lie, as well as intervals in which the respective values ​​of the vectors are located. values ​​of the vector and the vector

[0066] By way of example, step 501 includes searching, for example on another computer and upstream of the programming of the Kalman filter on 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- As an 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 then, for example, 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 executing optimization algorithms. The methods for finding such matrices are known and are within the grasp of a person skilled in the art.

[0069] In some cases, the vectors xk 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 time 4, the three vectors xk, uk, and uk 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 process continues in step 503 (Normalization of the state model matrices). As an example, step 503 is performed on a separate computer, prior to the programming of the Kalman filter on circuit 106. Step 503 involves finding two additional transformation matrices VT and W2 that normalize the matrices TaATa, TaBTb, and TAC. Indeed, step 501 is insufficient 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(TaBTb and W2TAC) is bounded, for example, at absolute value by a real number M, for example equal to 1.

[0072] The search for the transformation matrices and W2 is carried out by another or dinator, upstream of the programming of the Kalman filter on 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 negative.

[0073] The transformation matrices and W2 are, for example, then stored in non-volatile memory and / or in 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 VF3 and W2 transformation matrices is carried out, for example, by executing optimization algorithms. The methods for searching for such matrices are known and within the grasp of a 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 The dynamic state model 504 is then a fully normalized system. More specifically In this respect, 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 are respectively equal to A = W\TaATa' KB = W^T^Tb and C = W2TcC7^A, are normalized. The vectors 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 t. 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 linearization step of a nonlinear state model.

[0078] Following the completion of step 503, the process continues in a step 505 (Expression of the Kalman filter for the descriptor system). As an example, step 505 is carried out upstream of the programming of the Kalman filter on the circuit 106. In step 505, a Kalman filter descriptor 506 (Kalman filter for the 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 dynamic linear descriptor systems. Such a filter is described in more detail by R. Nikoukhak, AS Willsky, and BC Levy in the article "Kalman filtering and Riccati equations for descriptor systems" published in 1992 in IEEE Transactions on Automatic Control.

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

[0081] An estimate Pæ" application of a Kalman filter for a system re presented by a fully normalized linear dynamic descriptor 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 of 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 linear descriptor system for the dynamical equation system 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 dynamical system represented by a linear state-descriptor model as described above is therefore not systematically numerically stable.

[0084] Step 505 is, for example, performed 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, they make use, for example, of 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 executed by the processor 108, the instructions also make use, for example, of the normalized vectors, Ük and Ük. Furthermore, when executed by the processor 108, the instructions make use of covariance matrices P^ whose theoretical mathematical properties, including symmetry and being positive definite, are preferably verified at every time 4.

[0085] To ensure that the covariance matrices P^ remain symmetric and positive definite, they are obtained via a factored expression of the Kalman filter for dynamical descriptor system described by a linear state descriptor model, carried out in a step 507 (Factorization of the Kalman filter for fully normalized state descriptor 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 linear dynamical descriptor system are factored for each instant 4. Factoring the covariance matrices ensures that they are symmetric and positive definite for each instant 4. The numerical implementation of the Kalman filter for the fully normalized descriptor state model 506 in factored form ensures that the filter is numerically stable with positive definite covariance matrices P. Furthermore, the numerical implementation of this filter is feasible with fixed-point arithmetic on the processor 106, given the normalization steps performed upstream of the Kalman filter application on another processor. For example, the factorization of the covariance matrices is performed on the fly, at each instant 4.

[0087] The covariance matrices Pk+^+i and the estimates ,X / .-+^+î derive principally Matrix representation: [Math 6] y1 However, for each instant 4, this matrix is regular but indefinite. Using Schur's complement, the matrix - which by definition is negative definite - appears in the explicitation of the matrix La Since matrix Mk is undefined, the Cholesky decomposition allows 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., one with complex coefficients, Ut exists because the matrix Mk is symmetric. Therefore, we write: [Math 7] [V with: c = u^uk, 0 1 [Math 8] 3Jc Step 507 includes the search for matrices.

[0089]

[0090]

[0091]

[0092]

[0093]

[0094]

[0095] Indeed, following the development of the UkUk product, we obtain the following: [Math 9] (¾)7 = Uu = 0, (¾ fu4k=wt ( «u ) TUu + (¾) Tuik=M uk )Tu5k=cT, Uu+iU^k) U 5k + (U «:) ^151=0- Since matrices Q and Pty are 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. uf The application of QR decomposition is carried out, for example, by the execution of 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) frequently that the matrix U is the Cholesky factor of the matrix Rk, which is, by definition, definite positive. The expression for the matrix U4j< is obtained by solving a system of sorting equations angular formed by the matrices Similarly, the expression for the matrix U5k is obtained by solving a system of triangular equation formed by the matrices (¾)76^7 For example, systems of triangular equations are solved by substitution. For instance, the expressions for matrices U and U5I( are obtained following the execution of a substitution algorithm by processor 108. The matrices U and U5;.: are then stored in 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, i.e. such that j2 — - 1. Thus the product _ U^kU6k is such that - = VkVk = U^U^+U^U^- A QR decomposition then applies and allows us to obtain the following relationship: [Math 11] [L1 pRA LO J where r6 is an orthogonal matrix.

[0097] The QR decomposition is applied, for example, by executing 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 state descriptor model 506, in other words the expression: [Math 12] '0 we obtain that the matrix / >^ = -[0 oz]m à ' o = -[oo ijufu? o , LzJ Lz. The covariance matrix Pk+^c+ï is such that Pk+^+i = -VkV'k- Following its calculation, for example by processor 108, the matrix is ​​stored, for example, in non-volatile memory 110 and / or in 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 guaranteed numerically. It is also guaranteed that the numerical scheme implementing the application of the Kalman filter for a fully normalized descriptor state model 506 on the basis of the matrices thus obtained is numerically stable.

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

[0100] Following the factorization step 507, a factored Kalman filter for a system modeled by a descriptor state model and fully normalized 508 (Filter of (Kalman factored for a fully normalized descriptor state model) is obtained. This filter is initialized by a matrix that is the Cholesky factor of the initial covariance matrix p^ — the latter being symmetric by construction and defined positive, 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 non-volatile memory 110 and / or in volatile memory 112. In particular, at each instant ^+1, the updated estimate ^+1|^+1 of the states of the fully normalized state descriptor model is obtained for example 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 carried out, 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 Kalman Filter descriptor of step 506 are then described as steps carried out offline unlike step 508 which is carried out by the processor 108 upon receipt of an instruction to estimate the states of the fully normalized state model descriptor, on the basis of the factored Kalman filter for fully normalized state model descriptor 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 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 solution of the above equation is performed on the processor 108 of the circuit 106 using a propagation technique, in two steps, known by the person of the trade, 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 in the propagation technique consists of solving 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 in the propagation technique consists of determining the elements of a vector such that: [Math 18] U» 0 Therefore, we obtain that -3 - xk+ 1|A+1 “

[0107] The [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 for a windshield wiper. As an 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 tank. The physical state vector of the geared motor includes, for example, the position, velocity, and acceleration of the geared motor 601. In another example, the physical state vector of the geared motor includes, for example, the position determined by an angle, the angular velocity, 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- 601. As an example, sensor 602 is configured to perform measurements at regular time intervals, for example on the order of a few milliseconds. The times at which sensor 602 performs measurements are denoted, for example, 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 et 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 within the grasp of a person skilled in the art.

[0111] The sensor 602 is further configured to transmit the measurements taken at each instant 4 to a processor 604 (CPU). The processor 604 includes, for example, internal memory containing instructions 606 (Kalman Filter) allowing the application of a factored Kalman filter for the fully normalized descriptive system. In particular, the dynamic model describing the evolution of the physical states and measurements has been transformed, according to the embodiment described in relation to Figure 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 another computer. The modified parameters, i.e. the matrices A, B, C, Q.and are then calculated via this other device and are subsequently stored in a memory (not shown) connected to, or part of, the 604 processor.

[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 process described in relation to [Fig.5].

[0113] The processor 604 further includes 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 control 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] By way of example, the vectors ÿk} and Ük are stored volatilely and are for example deleted as soon as the vectors and Ùk+\- are received by the processor 604.

[0116] The internal memory of the processor 604 also stores for example 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 enable the generation of the updated estimate A7+]^+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 enable the application of a numerical scheme of a fully normalized, factored, descriptive Kalman filter as described in relation to [Fig. 5]. In particular, instructions 606 enable the implementation of a stable, fixed-point numerical scheme.

[0118] The 604 processor is further configured to transmit the estimated vector to the control circuit 608. The control circuit 608 then generates, based on 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 600 system and their programming is known and within the reach of a person skilled in the art.

[0120] One 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 guaranteed numerically. Furthermore, the calculations performed are fixed-point.

[0121] Various embodiments and variants have been described. A person skilled in the art will understand that certain features of these various embodiments and variants could be combined, and other variants will become apparent to a person skilled in the art. In particular, the search for the transformation matrices, described in connection with 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 a person skilled in the art. Moreover, the different parameters, such as matrices A, B, and C, of ​​the system of dynamic equations may be time-dependent. A person skilled in the art will then be able to adapt the normalization steps accordingly.Furthermore, matrices A, B, and C can be obtained by linearizing a nonlinear state model, leading to an extended Kalman filter to which steps 501 and 503 are applied, resulting in a fully normalized state-descriptor model, which allows the implementation of the numerical scheme of a factored Kalman filter for a fully normalized state-descriptor model. standardized.

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

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

Claims

1. Demands A method for estimating, by a processor, a vector x*+i comprising values ​​of physical states of a motorized device at a time tk+^k, the physical states including at least one indication of the device's position, the discrete-time evolution of the device's states 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 The 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 covariance matrix Q; the variable y comprises measurements, performed 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 R; and the matrices A^B and C are respectively evolution matrices of the physical states, the control inputs, and the measurements, the process comprising: - the normalization of the system of dynamic equations, the normalization including the search, by the processor, for a matrix W1 and a matrix W2 such that the matrices A = W1A, B — and C = W2C are bounded, and their storage in a memory; - the factorization of a Kalman filter descriptor having, for the moment tk, a covariance matrix Pkk. The factorization includes the calculation of a block triangular matrix Uk 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 memory storage of at least one of the matrices composing the block matrix Uk; and - the calculation, by the processor connected to the sensor and based on the vector comprising the measurements performed by the sensor, of an estimate of the vector x*+i based on the factored descriptor Kalman filter. Method according to claim 1, wherein the normalization of the

3.

4.

5.

6.

7.

8.

9. system of dynamic equations further includes, before the search for 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 etük = T cuk are bounded and in which the search for matrices and W2 is carried out so that the matrices A = WB = W}TABrB and C = W2TcCTi are bounded. A method according to claim 2, wherein the covariance matrix P^ is guaranteed to be numerically symmetric and positive definite, and wherein the calculation of the estimated xA.+)£+i comprises the execution, 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 according to 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. Method according to 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 depends on the time A method according to any one of claims 1 to 7, wherein at least one of the matrices A, B and / or C is obtained by nonlinear state model linearization. A method according to any one of claims 1 to 8, wherein the search, by a processor, for 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 estimated 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, whose evolution through one or more physical states 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 its covariance matrix a matrix Qk; the variable Vk comprising measurements, made by a sensor, of at least one of the physical states and / or at least an 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 its covariance matrix a matrix Rk; - a memory in which matrices A, B, and C are stored, along with matrices W1 and W2, such that the matrices A = W1A, B = W1B, and C = W2C are bounded, and for storing sets of matrices Rk and W1, where for each instant tk, ke ∈ N, W1 is the covariance matrix associated with W1, where Rk is the covariance matrix associated with W2vk, the memory further storing, in association with each instant tk, a covariance matrix P1^ such that uTkuk^ block ; - the control circuit configured to generate the uk control data for each tk instant; - 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 instant tk, the sensor being further , a triangular matrix Uk 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 command the storage, in memory, of the covariance matrix Pk+^+ï of the estimate ^+^+r

12. A system according to claim 11, wherein the control circuit is configured to generate control data based on the estimated

13. A system according to 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 matrices and W2 are such that the matrices A = W {TaAT-1, B = W {TaBT~b and c = W2TcC7^ are bounded, the processor being further configured for: - generate the vector ÿk on the basis of the matrix Tc; and - generate the vector ük on the basis of the matrix TB.

Images