Recursive weighted least squares estimation algorithm based on minimum model error principle

Xiaoyun Lei(雷曉云),Zhian Zhang(張志安)

School of Mechanical Engineering,Nanjing University of Science and Technology,Nanjing 210094,China

Keywords:Minimum model error Weighted least squares method State estimation Invariant embedding method Nonlinear recursive estimate

ABSTRACT Kalman filter is commonly used in data filtering and parameters estimation of nonlinear system,such as projectile"s trajectory estimation and control.While there is a drawback that the prior error covariance matrix and filter parameters are difficult to be determined,which may result in filtering divergence.As to the problem that the accuracy of state estimation for nonlinear ballistic model strongly depends on its mathematical model,we improve the weighted least squares method(WLSM)with minimum model error principle.Invariant embedding method is adopted to solve the cost function including the model error.With the knowledge of measurement data and measurement error covariance matrix,we use gradient descent algorithm to determine the weighting matrix of model error.The uncertainty and linearization error of model are recursively estimated by the proposed method,thus achieving an online filtering estimation of the observations.Simulation results indicate that the proposed recursive estimation algorithm is insensitive to initial conditions and of good robustness.

1.Introduction

Optimal estimation and prediction of projectile"s trajectory are the key points in trajectory control technologies.Usually the accuracy of state estimation is strongly dependent on the accuracy of ballistic mathematical model.In simple trajectory correction schemes[1,2],kinematic equations of projectile"s ballistic are generally used as the state equations.The projectile"s flight data can be detected by different measurement system schemes.For example,in autonomous trajectory correction scheme based on projectile-borne GPS system,the controller needs to process the observed data including velocity and position coordinate,and identify necessary parameters such as the trajectory inclination angle and azimuth angle[3].For two-dimensional trajectory correction scheme,the attitude of the projectile needs to be detected and six-degree-of-freedom nonlinear differential equations is used as the ballistic mathematical model,which means that the two-dimensional ballistic control is more complicated than one-dimensional trajectory correction,as a result,the calculation in process of trajectory prediction or parameter identification is more complicated[4-7].Known as an important quantity,the accuracy of measurement data has a significant impact on trajectory prediction and parameter identification,especially in the prediction of impact point deviation during trajectory correction,which will ultimately affect the correction accuracy of the projectile.

Because of the poor working environment,the projectile-borne measurement system is more likely to be polluted by unknown system noise and process noise,resulting in the errors in observations and parameters"identification.Generally,we can filter,smooth or estimate the observations optimally to eliminate abnormal data and reduce the measurement error;then we can obtain reliable observations.Common algorithms used for data processing or filtering are Kalman filter[8-11],least squares method[12-14],particle filter(PF)[15-19]and intelligent algorithms(such as neural networks,support vector machine,etc.)combined with conventional algorithms[20,21].

Briefly,the principle of Kalman filter is to compute gain matrix(K)by minimizing the error covariance matrix(Pk)between estimate values and true values[8-15].Considering a linear system,the gain matrix can be yielded by Eq.(1).

WhereHis the transformation matrix of measurement model;Ris the covariance matrix of measurement noise.We should notice that,to derive Eq.(1),it"s assumed that the measurement noise and system noise(the covariance matrix isQ)are both white noises(the mean value are zero.).The prediction error covariance matrix,which is the error covariance matrix of predicted valuesand true values(Xk),can be yielded by Eq.(2).

Eq.(1)indicates that,gainmatrixKincreaseswith the incr easingof P′k,as a result,the feedback willgainmoreweight.Ifis set to be zero,which means the predicted value is equal to true value,thenK=0,so the estimate is equal to the predicted value.It can be seen that the Kalman gainKis positively correlated with.IfQand the state transformation matrix is determined,will be proportional toPk.This means if the initial value ofPkis large,Kalman gain matrixKcalculated in the first step of the recursive estimate will also be large,indicating that the observation is more reliable.In addition,if the state model is not established correctly(Actually,in many cases the state model cannot be established precisely or correctly.),system noise is not only resulted from itself,but also contains errors of the established state model.If model error is larger,the prediction based on the model will be worse,which can be equivalent to the increase of system noise.The essence of the least squares method is to minimize the error between estimate value and true value.If data are sampled under the assumed condition of Gaussian distribution,then,according to maximum likelihood estimation,to maximize the joint probability of the samples is equivalent to minimize the error variance of the estimate values and the true values[8].Actually,compared with recursive least squares method,Kalman filter only has one more step of state transformation,which is to predict the state value(prior).Kalman filter can be regarded as a special case of recursive least squares methods[8].In practice,linear system does not exist,and most system models belong to nonlinear systems.In some cases where nonlinear system is very approximate to linear system,linear filtering method can be used to obtain satisfactory results.However,for some highly nonlinear systems,the linear estimation method is always unsatisfactory.

Conventional nonlinear filtering methods include Expanded Kalman filter(EKF)[10,11],PF[16,17,24],Unscented Kalman Filter(UKF)[22,23],and etc.EKF linearizes the nonlinear system,and then linear Kalman filter method is adopted for data processing.One of the drawbacks of linear Kalman method is that it may introduce system error caused by linearization,resulting in the deviation in estimate values.To reduce the impact of this problem,methods such as increasing the order of Taylor expansion of the system model are good choices,but at the same time,the computational burden is also increased.Kalman filtering algorithm is featured with sequential processing structure,and it is very sensitive to the current observations and strongly depends on the accuracy of the state space model.Consequently,if the model or observation has gross errors,it usually makes the estimation accuracy worse,and the filtering may even diverge.Since EKF depends on linearization to propagate the mean and covariance of states,the estimation of EKF may not be accurate when the system is seriously nonlinear.UKF is an approximate method to estimate the variance of the mean and covariance of random variable when nonlinear transformation is used.UKF is effective to reduce the linearization error.However,even if the estimation accuracy of UKF is greatly improved compared with EKF,UKF is still an approximate nonlinear estimation method.PF is a totally nonlinear estimator based on Bayesian estimation method,while,the good performance of PF is at the cost of a large increase of computational burden.Another disadvantage of PF is sample degradation[24].This problem will get worse if the observations are inconsistent with the measurement model.Sample degradation can be mitigated by increasing the number of particles,but it will quickly lead to a huge amount of computation and the degradation problem cannot be solved essentially.

Generally speaking,“Minimum variance”and“maximum likelihood”are the two most common criteria for various state estimation algorithms.In minimum variance estimation,the state estimate error covariance matrix is minimized.In maximum likelihood estimation,the maximum probability estimation of the state is obtained with the knowledge of observations.These two estimation criteria require models of system error and measurement error as prior knowledge,which are difficult to be determined in practice.For example,Kalman filter needs to make strong assumptions empirically about distribution models of system error and measurement error(usually regarding as white noises).But for applications that have strict requirements on computational performance and estimation efficiency,such as projectile"s trajectory control,the nonlinear ballistic model is simplified;the flying environment of the projectile is harsh,and the measurement system of the projectile is also vulnerable to disturbance.As a result,Kalman filter or least squares method is usually not effective enough because it relies on the accuracy of filter parameters strongly,and the computational cost of UKF and PF is too much to meet the timeliness.

The application background of the paper is the trajectory correction control system based on projectile-borne GPS.A recursive estimation algorithm is presented based on WLSM and minimum model error estimation(MMEE)[25-29]with the knowledge of observed data and measurement error covariance matrix(Measurement error covariance matrix is far more likely to be known accurately than the model of system error.).The proposed algorithm,a nonlinear recursive method of optimally estimating state,is derived by solving a two-point boundary value problem with the method of Riccati equation solution[30,31],invariant imbedding[32]or numerical method[33,34]to estimate the uncertain error and linearization error of the system model.We adopt gradient descent method to determine the weight matrix of model error,ensuring that state estimation meets the system constraints.Simulation results show that the proposed algorithm can be generalized to other nonlinear system filtering applications.

2.Recursive WLSM based on MMEE

2.1.Basic concepts

The concept of MMEE has been applied to many fields such as system identification[35],on-orbit calibration of Satellite"s mass center[36],and optimal control[37,38].In other words,we account for model errors by adding a to-be-determined unmodeled disturbance vector to the original state model.Then,to obtain the optimal estimate of state under the conditions of minimizing the unmodeled disturbance vector,the problem is addressed by the minimum principle in optimal control theory.In this case,the unmodeled disturbance includes system noise,linearization error of nonlinear system or other uncertainties.

MMEE differs from Kalman filtering,recursive least squares and other sequential data processing methods in aspect that data processing is based on batch,namely,current state estimation is dependent on historical observations.And measurement residual(the term ofin Eq.(5))should meet the constraint of measurement error covariance matrix(Eq.(5)).The principle of WLSM is to minimize the weighted sum of squares of the measurement residuals.If an observation has gross error,it is of no practical significance to minimize the measurement residuals because we do not trust this measurement value at all.Therefore,the measurement residuals are weighted in the cost function,namely the first term in Eq.(6),representing the weighted sum of squares of the measurement residuals.After we introduce MMEE algorithm to WLSM,the cost function should include the minimization of unmodeled disturbance term(d(t))which is the second item in Eq.(6).

Eq.(3)is a modified state model equation respect to a generic system whose state vector dynamics is modeled by the continuous nonlinear system of equations,whereX∈Rn*1is state vector,f∈Rn*1is mathematic model equations,andd(t)is the to-bedetermined vector.

Given a set of discrete state-observable measurements modeled by the linear or nonlinear system of equations as Eq.(4).Whereis the measurement set at time tk,gk∈Rm*1is the measurement model,andvk∈Rm*1is a Gaussian distributed random sequence with zero mean and known covarianceRk.

The optimal state estimate is determined assuming that consistent estimates of the state must match the available measurements with a residual error covariance which is approximately equal to the known measurement error covariance.This necessary condition is hereafter referred to as the“covariance constraint”.It is appropriate to compare the averaged measurement-minus-estimate residual error covariance with a single prescribed measurement-minus-truth error covariance as Eq.(5).Whereis the optimal estimate of state at time tk.

The cost function is minimized with respect tod(t)as Eq.(6).WhereWis a n-by-n weight matrix determined so as to satisfy the covariance constraint as described shortly.

According to minimum principle in optimal control theory,it can be solved to minimize the cost function respect to control variabledand optimize state sequenceXunder the constraint condition(5).It is familiar for us to solve a two-point boundary value problem with invariant imbedding method or Riccati equation solution,etc.The numerical solution of two-point boundary value problem for continuous nonlinear problem is much more complicated,especially the computational cost increases exponential with the increasing of system dimension.It is necessary to derive a recursive estimation algorithm to achieve the on-line state estimate of nonlinear system without the knowledge of model error.

2.2.Derivation of recursive WLSM based on MMEE

MMEE accomplishes a batch estimate via sequential processing of the measurements,which is not a sequential filtering[25].According to the concept of MMEE,we improve WLSM to recursively estimate the state in sequence on-line.

The system state space is modeled as Eq.(7),where k denotes timestep.

The difference-equation model is yielded as Eq.(8).

The to-be-determined termd(k)is added to the right-hand side of Eq.(7),yielding Eq.(9).WhereG(k)is a coefficient matrix.

The state-observable measurement is modeled by Eq.(10),whereh(.)is a measurement transformation matrix.

Compared with original nonlinear model,the system error is unknown.We choose the state variable as the observed vector,and establish a quadratic performance function as the cost function J,Eq.(11).The Hamiltonian function(H)is yielded as Eq.(12)according to minimum principle,Wis a weight matrix,andRkis covariance matrix with an assumption thatRkequalsR.

The control equation is Eq.(13).

Eq.(13)can be expanded as

Eq.(14)yields

The Hamilton"s canonical equations is as Eq.(16),whereλ(k)is the vector of co-states.

Eq.(17)is yielded by expanding the second equation in Eq.(16).

Eq.(18)is an equivalent form of Eq.(17).

With Eq.(15)and Eq.(18),the first equation in Eq.(16)can be rewritten as

The boundary conditions and constraints are

Where k=0,1,2…N-1.

Eqs.(18)-(20)compose a two-point boundary value problem(TPBVP)with constraints and knowledge of observations.

Assuming that Eqs.(18)and(19)can be written as the following embedded expressions.

The boundary conditions are embedded in Eq.(22),where N is arbitrary.As a result,Eq.(23)is yielded.

Where

Eq.(24)is equivalent to Eq.(28).

The solution"s format of Eq.(28)is as Eq.(29).

Eq.(28)can be simplified by Eq.(25),and yields Eq.(30)

The right-hand sides of Eq.(30)can be rewritten as

With Eq.(26),we know that a+Δa=η[T(a,k),a,k].So,with Eqs.(29)and(31),Eq.(30)can be simplified as

As to Eq.(32),we expandβandηat the point of a=0 with the method of Taylor expansion,and ignore the higher order term.Eq.(33)is yielded by Eq.(32).

Let the corresponding coefficients of pow a be equal.

Combined with Eq.(13)and Eq.(21),we make denotations as

With the denotations,we yield

And Eq.(34)is rewritten as

According to Eqs.(39)and(40),we know,

Then,we differentiate equations(42)and(43)with respect to a,

As a result,Eq.(35)is rewritten as

Eq.(46)is simplified as

The weight matrixWof system model error must be constrained by the covariance constraint of Eq.(20).If the constraint wasn"t met,Wshould be adjusted.We seek the smallestd(t)which is consistent statistically with the measurements.

2.3.wt matrix W

By method of solving the two-point boundary value problem of minimizing cost function J Eq.(11)respect tod(t)with constraints,the state vector can be estimated.The first term of cost function Eq.(11)is relevant to the inverse of measurement error covariance matrixR,with a smallR,accurate measurements are weighted more heavily;with a largeR,inaccurate measurements are weighted more heavily.The second term of Eq.(11)reflects the assumption that the amount of unmodeled effect or model error to be added should be minimized,i.e.,the original model should be adjusted by a minimal amount.The added term should narrow the difference between the estimate values and measurements when the original model has a large error.As a result,J decreases,while the added termd(t)increases the second term of J.The proper balance between the two competing effects depends on the choice of the weight matrixW,which is fit such that the covariance constraint Eq.(20)is satisfied.

Section 2.2 indicates that,with a proper choice ofW,the recursive estimate algorithm is effective in on-line sequence estimate.Otherwise,it is more preferable for a batch estimate.To achieve an on-line estimate,the weight matrixWshould be predicted first.Fig.1 shows a plot ofWversus the difference between measurement-minus-estimate variance(estimate variance)and the measurement-minus-truth(measurement variance)variance in one-dimension system model with a known measurement error variance R(R=2).Fig.1 indicates that a proper choice ofWminimizes the bias between measurement-minus-estimate variance and the measurement-minus-truth variance,resulting in an optimal estimate of state based on minimum system model error.Thus,the appropriate value forWis the one which allows enough model correction to make the measurement-minus-estimate variance match the measurement-minus-truth variance.The figure also shows that covariance constraint can be satisfied within a certain range ofW.Gradient descent algorithm is available to determine a proper value ofW.

With the covariance constraint,

Fig.1.The curve of W versus the difference between estimate variance and measurement variance.

2.4.Algorithm scheme of MMEE-WLSM

In summary,the flow of the proposed recursive estimate algorithm is demonstrated in Fig.2.The recursive estimation algorithm can be summarized as:

3.Simulation examples

3.1.VanderPol equation model

We take the VanderPol equation as the example 1 of nonlinear model.

The state vector isX=[x1;x2］,where x1=x";x2=x.So Eq.(51)can be decoupled as Eq.(52).

Fig.2.Recursive estimate algorithm Scheme.

The discrete system is modeled as

Assuming that Tx1（k）is regarded as the true model error(d(t)),so the state model with unknown model error d(t)is written as

As to EKF,we consider that the influence of unknown system model error is equivalent to the consequence of an increased system noise.Assuming that system noise is modeled as white noise,namely w～N(0,1).There also exist constant error c=3 and unknown system error d(t);and the measurement model noise is also considered as white noise v～N(0,1).The measurement model isz(k)=X(k)+v,which means the measurement matrix isH=I2＊2.The initial conditions of Kalman filter in this case are as follows:=［0.35;0.35］;the estimate covariance matrix is initialized asP(0)=[100 0;0 100].As to MMEE-WLSM,the measurement model is set as z(k)=x1+3+v,and the weight matrixWis[0.005 0;0 0.05].The estimated values of state x1under different estimation methods are obtained by simulation,illustrated in Fig.3.In Fig.3,the curve of MMEE-WLSM is smoother than Kalman filter compared with the curve of true values of x1.The curves of variances of estimate,calculated by,is shown in Fig.4.In Fig.4,the curve fluctuates greatly at the beginning,then,it converges rapidly,closing to variance of measurements.It means that the state estimation is carried out under the covariance constraints;therefore,the errors between the estimate values and true values are small.Fig.5 is the curves of estimate errors(the deviation between estimate values and true values)calculated by two methods.Fig.5 shows that the deviation of the estimate values obtained by the MMEE-WLSM algorithm is less than Kalman filter.

Fig.3.The estimated values of state x1.

Fig.4.The curves of the variances.

Fig.5.The curves of estimate errors.

3.2.State estimate of 2D ballistic model

According to section 3.1,MMEE_WLSM algorithm performs well in nonlinear state estimate and discrete system.Here,we apply it to ballistic model with unknown linear model error to verify its efficiency.As we know,in one-dimensional trajectory correction control[4],2D ballistic model can meet the requirements of simple trajectory correction.The nonlinear ballistic model is shown as Eq.(55).The discrete first-order difference-equation of trajectory model is as Eq.(56).Where S is the characteristic area of projectile with a unit of m2;ρis air density with unit of kg/m3;m is the mass of projectile with a unit of kg;g is the gravity constant;θis launch angle.Eq.(56)is used for MMEE-WLSM.

Let state vector beX=[v,θ,x,y]T.As to the discrete model,linearized error of the nonlinear trajectory model is unknown.Let measurement observed model be a linear model as Eq.(57).

Then,we can yield Eq.(58)-(64).

According to the proposed algorithm,

The weight matrixWis chosen to be

The initial true value is[110,25/180*pi,0,0]T.The initial estimate values are set as=[110,20/180*pi,0,0]T，P(0)=0.Fig.6 is the simulation results of MMEE-WLSM algorithm.Fig.6(a)is a plot of range vs.height of true values,measurements and estimate value,respectively.It can be seen from the detailed enlargement of the ascending section that the offsets between the estimated trajectory and real trajectory is much smaller than that between the measurement trajectory and ideal trajectory,which indicates that,with the constraints,the presented recursive algorithm can estimate the fight trajectory accurately.Fig.6(b)is the curves of estimate errors in range(the deviation between estimate values and true values)calculated by MMEE-WLSM and Kalman.MMEE-WLSM shows a better performance.With an increasing time,it converges nearly 0,while Kalman method leads an unstable convergence.Fig.6(c)-6(f)are the plots of estimate variances of the state vector components.The estimate variances of the four components are converged to the corresponding measurement variances.The results show that the state is estimated under the constrain conditions.From the figures,it is obvious that the initial state vector is deviated from true values,while it seems that this deviation affects the subsequent estimate values little,indicating that the algorithm is not sensitive to initial deviation.

3.3.State estimate of 3D ballistic model

3D ballistic model is a simplified model which can be used for trajectory prediction when lateral range is needed.The state model is nonlinear equations,as Eq.(68),where the parameters in Eq.(68)represent the same meaning with 2D ballistic model.z denotes lateral range,φdenotes direction angle(rad),vris relative velocity,wx,wyand wzare components of wind speed.Other symbols can be found in Ref.[4].Let state vector beX=[x,y,z,v,θ,φ]T.Let measurement observed model be a linear model as Eq.(69).

We initialize the matrixRwith diag(3,6,3,5,0.01,0.01),initial optimal state vector,weight matrixWwith diag(0.0000008,0.0000008,0.0000008,0.0000008,0.0000008,0.0000008),and the timestep is 0.01s.We linearize the nonlinear model by Taylor expansion method.G(t)is set to beI6＊6.d(t)represents discretization error of state and the uncertainty error of the system.The initial estimate state vector is set to be different with the true values.The true initial state vector is[0.918,0.917,0.0453,129,0.785,0.0349]T,and=[0.918,0.917,0.0453,129,0.780,0.0049]T.Simulation test is also done by Kalman method with the same initial state vector.Fig.7 is the plot of 3D trajectories.The estimated trajectory represented by a red dashed line is smooth and close to the ideal trajectory.While the measurements are polluted by noise.Fig.8 shows the convergences of variance of the state components.All variances converge to the corresponding measurement variances(R).The curves of estimate errors are shown in Fig.9.Actually,the initial state values impact the performance of Kalman,as we can see,the initial estimate values are unstable and rough.This stage lasts a longer time than MMEE-WLSM method.

Fig.6.Results of simulation example 2.

Fig.7.Trajectory of example 3.

Fig.8.Variances of example 3.

4.Conclusions

In this paper,a recursive estimate algorithm is presented based on minimum model error principles and weighted least squares method,which can estimate state optimally on-line.With the knowledge of measurements and measurement error covariance matrix,the method of solving two-point boundary value problems,invariant imbedding method,is adopted to derive the estimate algorithm,which can recursively estimate the uncertainty errors and the optimal state of discrete system or nonlinear system online sequentially.The algorithm is demonstrated for different examples,especially for projectile"s parameters estimate in external ballistics,and the results indicate that the algorithm is capable of obtaining accurate state estimates with good robustness and low demand for initial accuracy in the presence of poor modeled system.

Fig.9.Errors of example 3.

Data availability

The datasets and codes generated and analyzed during the current study are available in the Github repository,[https://github.com/raykking].

Declaration of competing interest

The authors declare that there are no conflicts of interest regarding the publication of this paper.

Acknowledgments

This work is supported by Postgraduate Research & Practice Innovation Program of Jiangsu Province(KYCX18_0467),Jiangsu Province,China.During the revision of this paper,the author is supported by China Scholarship Council(No.201906840021),China to continue some research related to data processing.