Auto-calibration Kalman ﬁlters for non-linear systems with direct

The problem of state estimation for non-linear systems with unknown inputs is discussed. The objective is to construct a non-linear ﬁlter where the unknown input affects both the state equation and measurement equation. An auto-calibration Kalman ﬁlter is proposed with three stages, where the ﬁrst and second stages are the extension of the two-stage Kalman ﬁlter in non-linear system cases, and the third stage is the extended Kalman ﬁlter with correlated noise. Compared with the two-stage extended Kalman ﬁlter, the presented algorithm conducts a more reasonable linearization of the measurement equation in the third stage. As a result, auto-calibration is realized and more accurate and more stable state estimation can be obtained. Simulative and practical examples both demonstrate the validity and superiority of the presented auto-calibration Kalman ﬁlter. Furthermore, positioning experiments illustrate that the auto-calibration Kalman ﬁlter can obtain an accurate long-term heading estimation in pedestrian navigation and is easy to apply in engineering applications.


INTRODUCTION
The problem of state estimation for systems with unknown inputs has been a research hotspot in the past decades [1]. As is well known, an underlying assumption of the classical Kalman filter is that the process and measurement disturbances can be accurately modelled [2]. However, in practical engineering unknown disturbances often emerge due to various reasons. Under such circumstances, the classical Kalman filter may produce large errors. Therefore, plenty of mitigation strategies have been presented in order to accurately estimate the state of systems with unknown inputs. For linear systems, the earliest methods were to augment the Kalman filter state vector with an unknown input vector [3], which was designated as bias terms. As a result, the augmented state Kalman filter (ASKF) can be utilized to estimate the state and unknown input simultaneously. However, when the number of bias terms is comparable to that of state variables of the original problem, there may be excessive computation with the ASKF. Furthermore, the accuracy and computational speed may be severely compromised [4]. To overcome this problem, Friedland [4]  ple the central filter into the "bias-free" filter and the "bias" filter. Afterwards, researchers have presented plenty of developments of the TSKF. Ignagni [5] utilized conventional Kalman estimation theory to rederive Friedland's [4] result and demonstrated equivalency between the ASKF and the TSKF. Besides, this derivation considered a class of problems where the state and unknown input may be initially correlated, whereas the main result in [4] is based on the assumption that they are initially uncorrelated. In [6], Ignagni addressed the class of problems in which the unknown input was driven by white noise uncorrelated with the system and measurement noise. The resultant decoupled estimator structure provided both increased computational efficiency and improved stability relative to the ASKF. In [7], Alouani et al. considered the case where the unknown input was driven by white Gaussian noise that was correlated with the system noise. Under an algebraic constraint regarding the noise above, the presented filter was equivalent to the ASKF and is optimal. Since the algebraic constraint is restrictive in practice, they also concluded that for most practical systems the presented two-stage estimator is suboptimal. In [8] and [9], Hsieh and Chen proposed an optimal two-stage Kalman filter (OTSKF) where the algebraic constraint in [7] was removed and the optimal performance was achieved. Furthermore, the equivalency between the ASKF and the OTSKF was proven.
On the other hand, to deal with the case that no prior information about unknown inputs is available [1], robust filters have been developed. Kitanidis [10] first presented an optimal recursive state filter which obtained unbiased minimumvariance linear state estimation. Later, Darouach and Zasadzinski [11] extended Kitanidis' result and founded stability and convergence conditions of the filter, and Kerwin and Prince [12] proved that the recursive framework in [10] and [11] obtained globally optimal estimations. Furthermore, Hou and Patton [13] derived an optimal filtering formula for linear time-varying discrete systems with unknown inputs by making use of the wellknown innovations filtering technique. In [14], Keller, Darouach and Caramelle presented a new method for state filtering in linear discrete-time stochastic systems with unknown inputs. The proposed unbiased minimum-variance estimator was used for robust decentralized state and constant bias filtering. In [15], Hsieh developed a robust two-stage Kalman filter (RTSKF) by utilizing the TSKF and OTSKF techniques and modifying the measurement update equations of the unknown-input filter. The RTSKF was equivalent to the Kitanidis' unbiased minimum-variance filter and obtained estimations of both the state and the unknown input. In addition, Gillijns and De Moor [3] proved that the estimation of the unknown input in the RTSKF is globally optimal. In [16], Darouach, Zasadzinski and Boutayeb constructed an optimal filter in the general case where the unknown input affected both the state and measurement equations. It extended the results in [11] where the unknown input was only present in the state equation. In [3], in order to simultaneously estimate the state and input of linear systems, Gillijns and De Moor developed a recursive filter where the state estimation and input estimation were interconnected. The input estimation was calculated from the innovation by leastsquares estimation and the state estimation was transformed into standard Kalman filtering. Furthermore, Gillijns and De Moor [17] extended their work in [3] to estimate the state and input of systems with direct feedthrough. In [17], the distribution matrix before the unknown input in the measurement equation must be of full column rank [1]. Cheng et al. [1] presented a state estimator where the requirement on the distribution matrix of the unknown input is milder than the one in [17] and proved the global optimality of the recursive filter. Hsieh [18] derived an optimal filter in the general case where the unknown input affected both the state and the output and the direct feedthrough matrix had arbitrary rank.
For non-linear systems, various researches have been conducted based on extension of filters for linear systems. In early times, Sinha and Mahalanabis [19] adopted the method in [4] to consider non-linear systems with input biases. In their work, the measurement equation was linear and there were no measurement biases. Shreve and Hedrick [20] dealt with state estimation in non-linear continuous systems with discrete measurements. In their study, both the state equation and measurement equation were non-linear and there were only sensor biases. Mendel [21] extended Friedland's [4] bias filtering technique to non-linear systems where states enter non-linearly but biases enter linearly. In his research, both the state and measurement equations were non-linear and affected by biases. In [22] and [23], Caglayan and Lancraft considered non-linear discrete-time systems with biases both in the state equation and measurement equation, where Friedland's [4] separate-bias estimation approach was generalized to the extended Kalman filter (EKF) formulation. The effectiveness of their method, the two-stage extended Kalman filter (TSEKF), was validated in aircraft navigation simulation.
In recent years, Xu et al. [24] designed a two-stage unscented Kalman filter using the adaptive fading unscented Kalman filter. Besides, the stability of the designed filter was analysed and ensured under certain conditions. Kim, Lee and Park [25] proposed an adaptive two-stage extended Kalman filter utilizing the adaptive fading extended Kalman filter and applied their filter to a loosely coupled system with an unknown fault bias. Zhang et al. [26] presented a two-stage cubature Kalman filter for high-dimensional non-linear systems with random bias. In comparison with the augmented state cubature Kalman filter, their filter avoided dimension disaster and had little computation.
As is introduced above, the problem of state estimation for non-linear systems with unknown inputs has continuously been one of the research hotspots in control and filtering field. However, due to the complexity in non-linearity, some problems remain unsolved. On the other hand, state estimation for systems with unknown inputs is significant in real-world problems [27,28]. An auto-calibration Kalman filter (ACKF) is proposed for non-linear discrete systems with direct feedthrough of the unknown input to the output. The proposed filter is divided into three subfilters, that is, the bias-free filter, the bias filter and the state filter. Simulation results show that an accurate and stable state estimation can be obtained utilizing our filtering method. Also, the ACKF is applied in pedestrian navigation field achieving a pretty good performance.
The rest of this paper is outlined as follows. Section 2 illustrates the problem formulation. The derivation of the ACKF is provided in Section 3. Simulative and practical examples are given in Sections 4 and 5, respectively. Section 6 makes a conclusion.

PROBLEM FORMULATION
The state estimation of the following non-linear time-varying discrete system will be given where x k ∈ R n , y k ∈ R p , w k−1 ∈ R n and v k ∈ R p are the state vector, measurement vector, process noise and measurement noise, respectively. d ∈ R m is the unknown input vector, which is constant. f k−1 (⋅) and h k (⋅) are non-linear vector functions. B k−1 and C k are deterministic known matrices with appropriate dimensions. w k−1 and v k are white Gaussian noise with following properties, where E is the symbol of mathematical expectation, Cov is the symbol of covariance, the superscript T denotes matrix transposition, and k j is the Kronecker delta function, which means The initial state x 0 is of mean ‚ x 0 and covariance P 0 , and is independent of w k and v k .
Applying the TSEKF proposed in [22,23], the state estimation of the system described above can be obtained as follows, where I is the identity matrix.

FILTER DESIGN
In the TSEKF described above, there are two linearization steps. In the first step, the state equation is linearized around This linearization is reasonable becausex k−1 is the best estimate of x k−1 after the measurement at time (k-1) is processed. In the second step, the measurement equation is linearized around is not the best estimate of x k before the measurement at time k is processed. Sinced k is a better estimate of d thand k−1 theoretically, the measurement equation is linearized around As a result, more accurate and more stable state estimation can be obtained compared with the TSEKF proposed in [22,23].
The ACKF contains three subfilters in three stages, respectively. Before the filter runs, the prior information of the unknown input is needed (i.e.d 0 and P d,0 ) and the initialization of the filter is described as follows: Stage 0: Initialization The first and the second stages of the ACKF are the extension of Friedland's [4] TSKF approach in non-linear system cases and are depicted as follows.
Stage 1: Bias-free filter First, based on the obtained estimates of the state and unknown input at the (k−1)-th step, a preliminary estimate of the state at the k-th step can be calculated as, Then Taylor series expansions of the state equation and measurement equation are performed around x k−1 =x k−1 and x k =x ′ k|k−1 , respectively.
where F k−1 and u k−1 are defined by the above equation and u k−1 can be treated as a known signal.
whereH k and z k are defined by the above equation and z k can be treated as a known signal.
Since Equations (19) and (20) are linear, the standard Kalman filter can be utilized to estimate the state. In bias-free filter, the unknown input d is ignored and the estimate of state can be obtained by the following Kalman filter recursive sequencē Stage 2: Bias filter First, applying the approach in [5], the following matrices are defined where G 0 = 0 Then the following measurement equation regarding d can be obtained where (H kxk|k−1 + z k ) can be treated as a known signal. j k is defined by the above equation and can be treated as measurement noise having the following covariance matrix [5,29] Considering that the unknown input is constant, its estimate can be obtained by the following Kalman filter recursive sequence After stages 1 and 2, the estimate of the unknown input,d k , is obtained. Therefore, the state and measurement equations can be rewritten as where d k is regarded as the known input and w ′ k−1 and v ′ k are regarded as new system and measurement noise, which are defined as It is noted that w ′ k−1 and v ′ k are correlated and (d −d k ) can be written as follows using Equation (32), Therefore, w ′ k−1 and v ′ k have the following covariance matrices, Applying the EKF with correlated noise [30], the third stage can be obtained as follows.
Stage 3: State filter Finally, the estimate of the state,x k , is obtained, and the proposed ACKF is given by Equations (16) and (17)

SIMULATION
To validate the accuracy and stability of the proposed ACKF method, a simulative example is conducted in this section. The

ALGORITHM 1 The proposed ACKF algorithm
Step 1: Initialization d 0 and P d,0 are given, k = 1 Step 2: Bias-free filter Step 3: Bias filter Step 4: State filter Step 5: k = k + 1 and return to Step 2 non-linear discrete-time stochastic system is given by, ( where d is the unknown input and d = −1. w k−1 and v k meet Equation (3), where Q k−1 = 0.01I and R k = 0.01. The total simulation time is 100 steps with the initial state x 1,0 ≈ N (0.4, 0.1 2 ) and x 2,0 ≈ N (0.4, 0.1 2 ). The prior information of the unknown input is d ≈ N (0, 1 2 ). A Monte Carlo simulation is performed and performances of the ACKF, the TSEKF and the EKF are compared. Figures 1 and 2 depict the state estimates using the above three filters in one single run. As shown, the state estimates using the ACKF and the TSEKF both fit the actual state well in this run. In contrast, the state estimate using the EKF quickly becomes zero in a few steps and maintains zero after that. This phenomenon is caused by the ignorance of the unknown input in the EKF. Therefore, the EKF fails to give an effective state estimation in this example. In the following analysis, the performances of the ACKF and the TSEKF are further compared.  Figures 3 and 4 depict the state estimates using the above three filters in another single run. As shown, the state estimate using the TSEKF quickly diverges and becomes unacceptable at the seventh step. Therefore, the TSEKF fails to give a stable state estimation in this example.
For further comparison, the root mean square error (RMSE) is adopted as the metric for evaluating filtering performance, which can be calculated by where j,k is the RMSE of the j-th element of state at the k-th step and N is the number of runs. x i j,k andx i j,k are the actual value and estimate of the j-th element of state at the k-th step in the i-th run, respectively.   Figures 5 and 6 depict the RMSEs of state estimates using the ACKF and the TSEKF in 500 runs, respectively. As shown, the RMSE of state estimates using the ACKF maintains at a low level from 0.09 to 0.15 and keeps stable. Whereas the RMSE of state estimates using the TSEKF quickly diverges and becomes unacceptable at the fifth step. This phenomenon is caused by the divergence of the TSEKF in some runs and illustrates that the TSEKF is unstable in this example.
Furthermore, the estimation of the unknown input can be conducted as well. As analysed in Section 3, the unknown input estimate can be calculated after Stages 1 and 2 of the ACKF. Figure 7 depicts the RMSEs of unknown input estimates utilizing the ACKF in the 500 runs. As shown, the RMSE converges to a steady-state value fast and maintains at a low level below 0.03. Therefore, the ACKF performs well in estimations of both the state and the unknown input. Table 1 lists the average RMSEs of state and unknown input estimates utilizing the ACKF in this example.
To summarize, the superiority of the ACKF has been validated in simulation and it has been proven that the ACKF has both high accuracy and stability.

APPLICATION
To further demonstrate the utility of the presented ACKF method, a practical example of pedestrian navigation is given in this section. In the current study, the tester walked around the stadium in Beihang University for one loop and recorded corresponding accelerations, angular velocities and magnetic flux densities. Then the location of the tester was determined utilizing pedestrian dead reckoning (PDR) approach.
As is well known, calculating an accurate long-term heading is the most challenging problem in PDR [31]. The following Kalman filter system is proposed to estimate the Euler angles between the carrier coordinate system and the navigation coordinate system, which is an improvement of the systems in [32,33].
The system model is constructed as, where is the heading angle, is the pitching angle, and is the roll angle. x , y , z are angular velocities observed in the carrier coordinate system. Δt k−1 is the sampling interval and w , w , w are noise. The measurement model is constructed as, where a x , a y , a z are accelerations observed in the  Through analysis of the recorded data, it is found that there is constant drift in tri-axial magnetic flux densities. Therefore, constant unknown inputs can be added to the last three equations in the measurement model described by Equation (51). The average heading angular errors in each step using approaches of the ACKF and the EKF are depicted in Figure 8. As shown, the angular error of the ACKF is closer to zero than that of the EKF in most steps. As a result, a more accurate long-term heading can be obtained applying the ACKF is presented.
The navigation results utilizing methods of the ACKF and the EKF are depicted in Figure 9, where the "Ref" represents RMSEs of unknown input estimates using the ACKF in 500 runs the actual trajectory which the tester walked along. Figure 10 illustrates the positioning errors in each step using approaches of the ACKF and the EKF. Table 2 lists the detailed positioning errors of the two algorithms.
As shown in Figure 9, the trajectory of the ACKF fits the actual one well while the trajectory of the EKF deviates from the actual one obviously. As shown in Figure 10, the positioning error of the ACKF is always smaller than that of the EKF. The biggest difference between the two algorithms is larger than 40 m. As listed in Table 2, positioning errors of the ACKF are much smaller than those of the EKF. Specifically, the average error of the ACKF is 4.5214 m, which is 16.45% of that of the EKF. The maximum error of the ACKF is 8.5389 m, which is 18.88% of that of the EKF. To summarize, the ACKF performs far better than the EKF and shows a satisfactory navigation performance.

CONCLUSION
A novel filter, the ACKF, which estimates the state of non-linear discrete-time systems with unknown inputs is proposed. In the presented filter, there are three subfilters in three stages, respectively. The first and second stages are the extension of Friedland's [4] TSKF approach in non-linear system cases, and the third stage is the EKF with correlated noise. In the third stage of the ACKF, a more reasonable estimate of the state is derived using a theoretically more accurate estimate of the unknown input, compared with Caglayan and Lancraft's [22,23] TSEKF algorithm. Therefore, more accurate and more stable state estimation can be obtained, and this is proven by simulation results. Furthermore, the presented ACKF can be applied in pedestrian navigation field and calculate an accurate long-term heading and precise location, which helps solve a conundrum in PDR.
Simulative and practical examples both validate the effectiveness and superiority of the ACKF. Besides, the proposed algorithm is easy to apply in engineering applications and has broad prospect. Future work will focus on state estimation for nonlinear systems with time-varying unknown inputs. Also, the performance of the ACKF in more engineering applications will be researched.