Central Difference Variational Filtering Based on Conjugate Gradient Method for Distributed Imaging Application

1. Introduction

With the development of airborne remote sensing system, its higher imaging resolution and three-dimensional imaging is increasingly arousing more interest in new research field, such as interferometric SAR (InSAR) and array antenna SAR imaging, where both locate multiantenna on different positions [20].Correspondingly, the motion compensation equipment is developed from a single POS to multiple in different locations with the assistance of ADPOS [1,5,20].In terms of system architecture of ADPOS, a integrated system collecting a high precision inertial measurement unit (IMU) and GNSS [6] are viewed the master POS, which integrates strapdown inertial solution and state correction to fuse the information of inertial and satellite sensors based on the linear model, while several low cost slave systems rely on fusing the information of the master and itself to improve parameter accuracy using various state estimation algorithms, then the whole system provides multinode high-frequency and high-accuracy motion parameters to the multinode loads for higher-resolution three-dimension imaging [7].

In order to compensate motion errors for multinode loads, ADPOS has to address the deterministic errors by calibration and stochastic errors using recursive state estimation. Comparing with deterministic errors, stochastic errors are more difficult when the inertial sensors for ADPOS are determined, therefore filter estimation algorithms play a vital role in the transfer alignment of DPOS, which has an direct impact on the recursive state estimation performance. The transfer model of the ADPOS in nature is nonlinear.However, earlierly the most-used KF is subject to the divergence that results from approximations during any linearization process [8,9]. And then EKF becomes the more widely applied estimator for the nonlinear model using via a first-order linearization method based on a suboptimal recursive estimation framework. However, EKF has some fatal shortcomings, dramatically degrades the accuracy and cause divergence of the state estimation when the nonlinear characteristic is very strong [3].

In order to overcome the shortcomings of above state estimation methods, then a series of nonlinear filtering algorithms are deduced to proceed nonlinear state estimation in the past years on the basis of the tradional state estimation algorithm [9]. Above these algorithms, several nonlinear state estimation are generated from deterministic sampling methods or numerical methods and both the linear minimum variance criterion is applied to the filtering update process [10], such as the widely used unsecnted Kalman filter [11], central difference Kalman filter [12] and so on. In view of that the algorithm flow is obtained by using the linear minimum variance criterion approximately, which is not the real case, then more efficient filtering update methods integrates the Kalman filter frame work need to be considered. nonlinear state estimation algorithms based on deterministic sampling using a new probabilistic framework are developed such as unscented Kalman filter (UKF) and central difference Kalman filter (CDKF) , where both rely on approximating the probability density distribution of the nonlinear function using a series of deterministic samples according to certain principles [4]. For the advantages, these state estimation algorithm processes remain free of Jacobian matrix calculation and truncation error to deal with nonlinear estimation problems [20], then higher estimation accuracy is achieved. Also, compared with the UKF algorithm, the CDKF algorithm uses fewer parameters to generate sigma points to conduct nonlinear estimation but the same estimation accuracy, which is the most widely used in nonlinear estimation [25]. However, the transfer alignment model is more high dimentional, CDKF and modified CDKF cannot fill the estimation accuracy requirement. How to deal with CDKF to achive better performance is very important once the high-dimention model is given.

For the filtering update process, one can treat it as an unconstrained optimization problem to be solved. Among of these fields, the steepest descent algorithm (gradient descent algorithm), Newton method and related Quasi-Newton method, conjugate gradient algorithm are used widely [13,14]. Conjugate gradient algorithm lies between the steepest descent algorithm and Newton method, which only employs the first order derivative information and overcome the defect of slow convergence of steepest descent algorithm, also is immune to the calculation and storage of Hessian matrix. Because of simple structure, smaller storage capacity, high numerical efficiency, conjugate gradient method has been one of the most efficient methods to solve the nonlinear optimization problem and used in many fields. Developed from Hestenes and Stiefel in the 1950s, conjugate gradient methods has been studied by many researchers [15], there are many derivatives developed from the basic methods, such as Hestenes-Steifel (HS) method [16], Fletcher-Reeves (FR) method [17], Polak-Ribiére-Polyak (PRP) method [18], conjugate descent (CD) method [19], Liu- Storey (LS) method [21], Dai-Yuan (DY) method [22] and so on, which differs from others in the selection of step length factors. Specially, the FR, CD, DY method performs well in convergence performance, but poor in numerical performance; while the PRP, LS, HS methods can realize self-correcting when encounter small step size continuously, then show better numerical performance. Here we focus on the PRP method to conduct the conjugate gradient to achieve optimization.

Compared with general filtering estimation methods and its related derivatives which use linear minimum variance criterion to assume that the state estimation is a linear regression with the observation [23], variational optimization is an effective method to replace the Kalman linear regression by quadratic terms about observation to nonlinearities [24], such as combine the conjugate gradient method with variational optimization to state mean estimation, which can achieve higher state estimation accuracy. On the other hand, for error covariance matrix estimation, the original update by general nonlinear filtering is used to estimate the state estimation covariance matrix, which takes place in parallel with state update. Motivated by the above description, based on the superiority of variational optimization by conjugate gradient method and the central difference Kalman filter adopts fewer parameters to achieve estimation, a central difference based variational filtering algorithm (CDVF) is proposed to contribute to the information fusion process of the master and slave systems of ADPOS. The main contributions of this paper can be summarized as the following aspects.

1) For the filtering prediction stage, the CDVF algorithm uses the Central difference Kalman filtering interpolation method to substitute derivative computation to propagate the system characteristic;

2) For the filtering update stage, the CDVF algorithm uses variational optimization by conjugate gradient method to conduct the state update instead of the linear minimum variance estimation; Meanwhile, the state estimation covariance matrix update is to be in progress parallel with the state update by the original central difference Kalman filtering;

3) Real flight test is conducted to verify the proposed algorithm, detailed experiment results are presented and show superiority of the proposed algorithm.

The outline of the reminder of this work is arranged as follows: In Section 2, the background theories including linear minimum variance criterion, central difference Kalman filtering algorithm, conjugate gradient optimization method are listed successively; Compare with the linear minimum variance criterion based central difference Kalman filtering algorithm, combine the central difference interpolation method, variational optimization method by conjugate gradient method, the central difference variational filtering algorithm is proposed in Section 3; The detailed experiment validation process including experiment design, data processing, result analysis are located in Section 4; Finally, the conclusion and future work are displayed in the last section.

2. Related Theories

In this section, the related theories including linear minimum variance criterion for nonlinear systems and developed nonlinear central difference Kalman filtering, conjugate gradient method for variational optimization will be displayed successively to serve for the next section.

Given the nonlinear system with additive noise as

$$\left\{\begin{array}{c}{\mathit{x}}_k=\mathit{f}\left({\mathit{x}}_{k-1}\right)+{\mathit{G}}_{k-1}{\mathit{w}}_{k-1}\hfill \\ {\mathit{y}}_k={\mathit{H}}_k{\mathit{x}}_k+{\mathit{v}}_k\hfill \end{array}\right.$$

(1)

where $k\in \mathbb{Z}$, ${\mathit{x}}_k\in {\mathbb{R}}^n$ denotes the state vector at discrete-time k, ${\mathit{y}}_k\in {\mathbb{R}}^{n_{\mathit{y}}}$ denotes the measurement vector at time k, $\mathit{f}(·):{\mathbb{R}}^n⟶{\mathbb{R}}^n$ is the nonlinear state transition function, ${\mathit{G}}_{k-1}\in {\mathbb{R}}^{n\times n_{\mathit{w}}}$ is the process noise coefficient matrix, ${\mathit{H}}_k:{\mathbb{R}}^n⟶{\mathbb{R}}^{n_{\mathit{y}}}$ is the measurement matrix, ${\mathit{w}}_{k-1}\in {\mathbb{R}}^{n_{\mathit{w}}}$, ${\mathit{v}}_k\in {\mathbb{R}}^{n_{\mathit{y}}}$ are mutually independent noise vectors, and $E\left[{\mathit{w}}_{k-1}{\mathit{w}}_{k-1}^T\right]={\mathit{Q}}_{k-1}$, $E\left[{\mathit{V}}_k{\mathit{V}}_k^T\right]={\mathit{R}}_k$.

2.1. Linear Minimum Variance (LMV) Criterion for Nonlinear Systems [23]

Let $\mathit{x}$ is the variable to be estimated, $\mathit{y}$ is the observation of $\mathit{x}$, if the following equality holds for ${\widehat{\mathit{x}}}_L\left(\mathit{y}\right)=\mathit{A}\mathit{y}+\mathit{b}$,

$$E\left[{(\mathit{x}-\widehat{\mathit{x}})}^T(\mathit{x}-\widehat{\mathit{x}})\right]{|}_{\widehat{\mathit{x}}={\widehat{\mathit{x}}}_L\left(\mathit{y}\right)}=\min$$

(2)

then ${\widehat{\mathit{x}}}_L\left(\mathit{y}\right)$ is the linear minimum variance estimation of $\mathit{x}$ on $\mathit{y}$, also can be denoted as $E^{\ast }[\mathit{x}/\mathit{y}]$. Also, $\mathit{A}$ and $\mathit{b}$ can be calculated as

$$\mathit{A}=C_{\mathit{xy}}{C}_{\mathit{y}}^{-1},\mathit{b}=E\left[\mathit{x}\right]-C_{\mathit{xy}}{C}_{\mathit{y}}^{-1}E\left[\mathit{y}\right]$$

(3)

then the linear minimum variance estimation of $\mathit{x}$ based on $\mathit{y}$ is formulated as

$${\widehat{\mathit{x}}}_L\left(\mathit{y}\right)=E\left[\mathit{x}\right]+C_{\mathit{xy}}{C}_{\mathit{y}}^{-1}(\mathit{y}-E\left[\mathit{y}\right])$$

(4)

and the estimation error covariance matrix is

$$\mathit{P}=C_{\mathit{xx}}-C_{\mathit{xy}}{C}_{\mathit{y}}^{-1}{C}_{\mathit{yx}}$$

(5)

For nonlinear systems as (1), let ${\mathit{y}}_{1:k}=[{\mathit{y}}_1,{\mathit{y}}_2,...,{\mathit{y}}_k]$, the linear minimum variance estimation of $\mathit{x}$ based on ${\mathit{y}}_{1:k}$ can be displayed as

$${\widehat{\mathit{x}}}_k=E^{\ast }[{\mathit{x}}_k/{\mathit{y}}_{1:k}]=E^{\ast }[{\mathit{x}}_k/{\mathit{y}}_{1:k-1},{\mathit{y}}_k]$$

(6)

According to the linearity of linear minimum variance estimation, it has

$$\begin{array}{cc}\hfill {\widehat{\mathit{x}}}_k& =E^{\ast }[{\mathit{x}}_k/{\mathit{y}}_{1:k-1}]+E^{\ast }[{\mathit{x}}_k/{\mathit{y}}_k]-E\left[{\mathit{x}}_k\right]\hfill \\ \hfill & ={\widehat{\mathit{x}}}_k^{-}+E^{\ast }[{\mathit{x}}_k/{\mathit{y}}_k]-E\left[{\mathit{x}}_k\right]\hfill \end{array}$$

(7)

On the basis of (3),

$$\begin{array}{cc}\hfill E^{\ast }[{\mathit{x}}_k/& {\mathit{y}}_k]=E\left[{\mathit{x}}_k\right]+C_{\mathit{xy}}{C}_{\mathit{y}}^{-1}({\mathit{y}}_k-E\left[{\mathit{y}}_k\right])\hfill \\ \hfill =& E\left[{\mathit{x}}_k\right]+E\left[({\mathit{x}}_k-E\left[{\mathit{x}}_k\right]){({\mathit{y}}_k-E\left[{\mathit{y}}_k\right])}^T\right]\hfill \\ \hfill ·& {\left\{E\left[({\mathit{y}}_k-E\left[{\mathit{y}}_k\right]){({\mathit{y}}_k-E\left[{\mathit{y}}_k\right])}^T\right]\right\}}^{-1}({\mathit{y}}_k-E\left[{\mathit{y}}_k\right])\hfill \end{array}$$

(8)

Let

$$\begin{array}{cc}\hfill {\mathit{P}}_{xy}& =E\left[({\mathit{x}}_k-E\left[{\mathit{x}}_k\right]){({\mathit{y}}_k-E\left[{\mathit{y}}_k\right])}^T\right]\hfill \\ \hfill {\mathit{P}}_y& =E\left[({\mathit{y}}_k-E\left[{\mathit{y}}_k\right]){({\mathit{y}}_k-E\left[{\mathit{y}}_k\right])}^T\right]\hfill \\ \hfill {\mathit{K}}_k& ={\mathit{P}}_{xy}{\mathit{P}}_y^{-1}\hfill \end{array}$$

(9)

then

$$E^{\ast }[{\mathit{x}}_k/{\mathit{y}}_k]=E\left[{\mathit{x}}_k\right]+{\mathit{K}}_k({\mathit{y}}_k-E\left[{\mathit{y}}_k\right])$$

(10)

Approximate $E\left[{\mathit{x}}_k\right]$ as ${\widehat{\mathit{x}}}_k^{-}$, $E\left[{\mathit{y}}_k\right]$ as ${\widehat{\mathit{y}}}_k^{-}$, then combine (7), one can check that

$${\widehat{\mathit{x}}}_k={\widehat{\mathit{x}}}_k^{-}+{\mathit{K}}_k({\mathit{y}}_k-{\widehat{\mathit{y}}}_k^{-})$$

(11)

and

$$\begin{array}{cc}\hfill {\mathit{P}}_{xy}& =E\left[({\mathit{x}}_k-{\widehat{\mathit{x}}}_k^{-}){({\mathit{y}}_k-{\widehat{\mathit{y}}}_k^{-})}^T\right]\hfill \\ \hfill {\mathit{P}}_y& =E\left[({\mathit{y}}_k-{\widehat{\mathit{y}}}_k^{-}){({\mathit{y}}_k-{\widehat{\mathit{y}}}_k^{-})}^T\right]\hfill \end{array}$$

(12)

Considering that ${\mathit{P}}_{yx}={\mathit{P}}_{xy}^T$ and ${\mathit{P}}_y$ is symmetric, then formula (4) is realized as

$${\mathit{P}}_k={\mathit{P}}_k^{-}-{\mathit{P}}_{xy}{\mathit{P}}_y^{-1}{\mathit{P}}_{xy}^T={\mathit{P}}_k^{-}-{\mathit{K}}_k{\mathit{P}}_y{\mathit{K}}_k^T$$

(13)

By the above deduction, the linear minimum variance estimation for nonlinear systems is formulated, but only the approximate results are achieved because of the nonlinearity of the system, then the different nonlinear filtering or smoothing algorithms are devoted to using different numerical calculation methods to derive ${\widehat{\mathit{x}}}_k^{-}$, ${\mathit{P}}_{xy}$ and ${\mathit{P}}_y$.

2.2. Central Difference Kalman Filtering

For system (1), under the assumption of Gaussian distribution, apply the linear minimum variance criterion and the Gaussian weighted integrals to the nonlinear filtering equation, the Gaussian filtering is obtained [10]. Especially, Stirling’s interpolation approximation generates the central difference Kalman filtering and is used to solve the numerical integrals to complete the recursive estimation [12].

By the above description, the central difference Kalman filtering algorithm in each step for system (1) is listed as:

• At time $k-1$, the Gaussian posterior density function is $p\left({\mathit{x}}_{k-1}\right|{\mathit{y}}_{1:k-1})=\mathbb{N}({\widehat{\mathit{x}}}_{k-1},{\mathit{P}}_{k-1})$, where ${\widehat{\mathit{x}}}_{k-1}=E\left[{\mathit{x}}_{k-1}\right|{\mathit{y}}_{1:k-1}]$ and ${\mathit{P}}_{k-1}=E\left[({\mathit{x}}_{k-1}-{\widehat{\mathit{x}}}_{k-1}){({\mathit{x}}_{k-1}-{\widehat{\mathit{x}}}_{k-1})}^T\right|{\mathit{y}}_{1:k-1}]$.

• Determine the set of sigma points ${\mathit{\chi }}_{i,k-1}$ according to

$$\begin{array}{cc}\hfill & {\mathit{\chi }}_{0,k-1}={\widehat{\mathit{x}}}_{k-1}\hfill \\ \hfill & {\mathit{\chi }}_{l,k-1}={\widehat{\mathit{x}}}_{k-1}+\upsilon {\left({\mathit{S}}_{k-1}\right)}_l\hfill \\ \hfill & {\mathit{\chi }}_{l+n,k-1}={\widehat{\mathit{x}}}_{k-1}-\upsilon {\left({\mathit{S}}_{k-1}\right)}_l\hfill \end{array}$$

(14)

where ${\mathit{P}}_{k-1}={\mathit{S}}_{k-1}{\mathit{S}}_{k-1}^T$, $i=0,1,...,2n,l=1,2,...,n$, ${\mathit{S}}_{k-1}=\mathrm{chol}\left({\mathit{P}}_{k-1}\right)$, $\upsilon =\sqrt{3}$.

• Propagate the set of sigma points in accordance with the system equation as ${\mathit{\chi }}_{i,k}^{-}=\mathit{f}\left({\mathit{\chi }}_{i,k-1}\right)$ and the predictive probability density function (pdf) $p\left({\mathit{x}}_k\right|{\mathit{y}}_{1:k-1})$ is approximated by the Gaussian pdf $\mathbb{N}({\widehat{\mathit{x}}}_k^{-},{\mathit{P}}_k^{-})$, where

$${\widehat{\mathit{x}}}_k^{-}=\sum _{i=0}^{2n}{\omega }_i^0{\mathit{\chi }}_{i,k}^{-}$$

(15)

$$\begin{array}{cc}\hfill & {\mathit{P}}_k^{-}=\sum _{l=1}^n[{\omega }_l^1({\mathit{\chi }}_{l,k}^{-}-{\mathit{\chi }}_{l+n,k}^{-}){({\mathit{\chi }}_{l,k}^{-}-{\mathit{\chi }}_{l+n,k}^{-})}^T+{\omega }_l^2\hfill \\ \hfill & ·({\mathit{\chi }}_{l,k}^{-}+{\mathit{\chi }}_{l+n,k}^{-}-2{\mathit{\chi }}_{0,k}^{-}){({\mathit{\chi }}_{l,k}^{-}+{\mathit{\chi }}_{l+n,k}^{-}-2{\mathit{\chi }}_{0,k}^{-})}^T]\hfill \\ & +{\mathit{G}}_{k-1}{\mathit{Q}}_{k-1}{\mathit{G}}_{k-1}^T\hfill \end{array}$$

(16)

where ${\omega }_0^0={\displaystyle \frac{{\upsilon }^2-n}{{\upsilon }^2}},{\omega }_l^0={\omega }_{l+n}^0={\displaystyle \frac{1}{2{\upsilon }^2}},{\omega }_l^1={\displaystyle \frac{1}{4{\upsilon }^2}},{\omega }_l^2={\displaystyle \frac{{\upsilon }^2-1}{4{\upsilon }^2}}$.

• Determine the set of predictive sigma points ${\mathit{\vartheta }}_{i,k}$ at time instant k according to

$${\mathit{\vartheta }}_{0,k}={\widehat{\mathit{x}}}_k^{-},{\mathit{\vartheta }}_{l,k}={\widehat{\mathit{x}}}_k^{-}+\upsilon {\left({\mathit{S}}_k^{-}\right)}_l,{\mathit{\vartheta }}_{l+n,k}={\widehat{\mathit{x}}}_k^{-}-\upsilon {\left({\mathit{S}}_k^{-}\right)}_l$$

(17)

and ${\mathit{P}}_k^{-}={\mathit{S}}_k^{-}{\left({\mathit{S}}_k^{-}\right)}^T$.

• Update the state estimation, compute the posterior pdf $p\left({\mathit{x}}_k\right|{\mathit{y}}_{1:k})=\mathbb{N}({\widehat{\mathit{x}}}_k,{\mathit{P}}_k)$ with the measurement ${\mathit{y}}_k$:

$${\mathbf{\Xi }}_{i,k}={\mathit{H}}_k{\mathit{\vartheta }}_{i,k}$$

(18)

$${\widehat{\mathit{y}}}_k^{-}=\sum _{i=0}^{2n}{\omega }_i^0{\mathbf{\Xi }}_{i,k}$$

(19)

$$\begin{array}{cc}\hfill {\mathit{P}}_y& =\sum _{l=1}^n[{\omega }_l^1({\mathbf{\Xi }}_{l,k}-{\mathbf{\Xi }}_{l+n,k}){({\mathbf{\Xi }}_{l,k}-{\mathbf{\Xi }}_{l+n,k})}^T+{\omega }_l^2({\mathbf{\Xi }}_{l,k}\hfill \\ & +{\mathbf{\Xi }}_{l+n,k}-2{\mathbf{\Xi }}_{0,k}\left){({\mathbf{\Xi }}_{l,k}+{\mathbf{\Xi }}_{l+n,k}-2{\mathbf{\Xi }}_{0,k})}^T\right]+{\mathit{R}}_k\hfill \end{array}$$

(20)

$${\mathit{P}}_{xy}=\sum _{l=1}^n\sqrt{{\omega }_l^1}{\mathit{S}}_k^{-}{[{\mathbf{\Xi }}_{l,k}-{\mathbf{\Xi }}_{l+n,k}]}^T$$

(21)

$${\mathit{K}}_k={\mathit{P}}_{xy}{\mathit{P}}_y^{-1}$$

(22)

$${\widehat{\mathit{x}}}_k={\widehat{\mathit{x}}}_k^{-}+{\mathit{K}}_k({\mathit{y}}_k-{\widehat{\mathit{y}}}_k^{-})$$

(23)

$${\mathit{P}}_k={\mathit{P}}_k^{-}-{\mathit{K}}_k{\mathit{P}}_y{\mathit{K}}_k^T$$

(24)

In consideration of that the measurement equation of (1) is linear, then the covariance matrix update of central difference Kalman filtering can be simplified as

$$\begin{array}{cc}\hfill {\mathit{P}}_y& =\sum _{l=1}^n[{\omega }_l^1({\mathbf{\Xi }}_{l,k}-{\mathbf{\Xi }}_{l+n,k}){({\mathbf{\Xi }}_{l,k}-{\mathbf{\Xi }}_{l+n,k})}^T+{\omega }_l^2({\mathbf{\Xi }}_{l,k}\hfill \\ & +{\mathbf{\Xi }}_{l+n,k}-2{\mathbf{\Xi }}_{0,k}\left){({\mathbf{\Xi }}_{l,k}+{\mathbf{\Xi }}_{l+n,k}-2{\mathbf{\Xi }}_{0,k})}^T\right]+{\mathit{R}}_k\hfill \\ \hfill & ={\mathit{H}}_k{\mathit{P}}_k^{-}{\mathit{H}}_k^T+{\mathit{R}}_k\hfill \end{array}$$

(25)

$${\mathit{P}}_{xy}=\sum _{l=1}^n\sqrt{{\omega }_l^1}{\mathit{S}}_k^{-}{[{\mathbf{\Xi }}_{l,k}-{\mathbf{\Xi }}_{l+n,k}]}^T={\mathit{P}}_k^{-}{\mathit{H}}_k^T$$

(26)

$${\mathit{K}}_k={\mathit{P}}_{xy}{\mathit{P}}_y^{-1}={\mathit{P}}_k^{-}{\mathit{H}}_k^T{({\mathit{H}}_k{\mathit{P}}_k^{-}{\mathit{H}}_k^T+{\mathit{R}}_k)}^{-1}$$

(27)

$${\mathit{P}}_k={\mathit{P}}_k^{-}-{\mathit{K}}_k{\mathit{P}}_y{\mathit{K}}_k^T={\mathit{P}}_k^{-}-{\mathit{P}}_k^{-}{\mathit{H}}_k^T{({\mathit{H}}_k{\mathit{P}}_k^{-}{\mathit{H}}_k^T+{\mathit{R}}_k)}^{-1}{\mathit{H}}_k{\mathit{P}}_k^{-}$$

(28)

By the above deduction, one can simply the filtering covariance matrix update, then it can be in progress in parallel with state update by using the one-step prediction ${\mathit{P}}_k^{-}$ and measurement matrix ${\mathit{H}}_k$, measurement noise covariance ${\mathit{r}}_k$.

2.3. Conjugate Gradient Method for Unconstrained Optimization Problems

For the unconstrained optimization problems, different algorithms are available, such as the steepest descent method, Newton and quasi-Newton methods, conjugate gradient method, etc. Compared with the linear minimum variance criterion which uses the linear method to complete the filtering update, the conjugate gradient method can be used to improve the filtering update process of the filtering algorithms.

Conjugate gradient method is very efficient for solving large-scale unconstrained optimization problem, detailed description is presented as follows. For the unconstrained optimization problem (matrix $\mathit{A}$ is positive definite)

$$\mathrm{F}\left(\mathit{x}\right)={\displaystyle \frac{1}{2}}{\mathit{x}}^T\mathit{A}\mathit{x}-{\mathit{x}}^T\mathit{b}$$

(29)

then $\mathit{r}\left(\mathit{x}\right)=\nabla \mathrm{F}\left(\mathit{x}\right)$ denotes the gradient of $\mathrm{F}\left(\mathit{x}\right)$, ${\nabla }^2\mathrm{F}\left(\mathit{x}\right)=\mathit{A}$. Here we will show the algorithm flow of conjugate gradient method. The most important point is to decide the step length factor $\mathit{\beta }$, popular algorithms such as FR, PRP, HS, DY and so on, the difference lies on the selection of $\mathit{\beta }$, here we list the algorithm flow of PRP conjugate gradient method as following [18].

Algorithm 1:Conjugate gradient method (PRP)

Initialization: ${\mathit{r}}_1=\nabla \mathrm{F}\left({\mathit{x}}_1\right)=\mathit{A}{\mathit{x}}_1-\mathit{b}$, ${\mathit{p}}_1={\mathit{r}}_1$, $m=1$

1: ${\mathit{\alpha }}_m={\displaystyle \frac{-{\mathit{r}}_m^T{\mathit{p}}_m}{{\mathit{p}}_m^T\mathit{A}{\mathit{p}}_m}}$

2: ${\mathit{x}}_{m+1}={\mathit{x}}_m+{\mathit{\alpha }}_m{\mathit{p}}_m$

3: ${\mathit{r}}_{m+1}=\mathit{A}{\mathit{x}}_{m+1}-\mathit{b}$

4: ${\mathit{\beta }}_{m+1}={\displaystyle \frac{{\mathit{r}}_{m+1}^T({\mathit{r}}_{m+1}-{\mathit{r}}_m)}{{\mathit{r}}_m^T{\mathit{r}}_m}}$

5: ${\mathit{p}}_{m+1}=-{\mathit{r}}_{m+1}+{\mathit{\beta }}_{m+1}{\mathit{p}}_m$

3. Central Difference Variational Filtering Algorithm

On the basis of the above subsections, the central difference variational filtering algorithm is developed, where the central difference Kalman filtering algorithm to conduct the state prediction, covariance matrix prediction and update, and the conjugate gradient method to update the state instead the LMV criterion. Combine the algorithm flow of central difference Kalman filtering and conjugate gradient method, here we present the outline of central difference variational filtering algorithm as follows [24].

Step 1: Sigma points generation. At time instant $k-1$, generate sigma points ${\mathit{\chi }}_{i,k-1}$ as (14).

Step 2: Prediction. Conduct (15) and

$$\begin{array}{cc}\hfill {\tilde{\mathit{S}}}_k^{-}& =[\sqrt{{\omega }_1^1}({\mathit{\chi }}_{1,k}^{-}-{\mathit{\chi }}_{1+n,k}^{-})\sqrt{{\omega }_2^1}({\mathit{\chi }}_{2,k}^{-}-{\mathit{\chi }}_{2+n,k}^{-})\hfill \\ \hfill & ...\sqrt{{\omega }_n^1}({\mathit{\chi }}_{n,k}^{-}-{\mathit{\chi }}_{2n,k}^{-})\sqrt{{\omega }_1^2}({\mathit{\chi }}_{1,k}^{-}+{\mathit{\chi }}_{1+n,k}^{-}-2{\mathit{\chi }}_{0,k}^{-})\hfill \\ \hfill & \sqrt{{\omega }_2^2}({\mathit{\chi }}_{2,k}^{-}+{\mathit{\chi }}_{2+n,k}^{-}-2{\mathit{\chi }}_{0,k}^{-})\hfill \\ \hfill & ...\sqrt{{\omega }_n^2}({\mathit{\chi }}_{n,k}^{-}+{\mathit{\chi }}_{2n,k}^{-}-2{\mathit{\chi }}_{0,k}^{-}){]}^T\hfill \end{array}$$

(30)

$${\mathit{P}}_k^{-}={\tilde{\mathit{S}}}_k^{-}{\left({\tilde{\mathit{S}}}_k^{-}\right)}^T+{\mathit{G}}_{k-1}{\mathit{Q}}_{k-1}{\mathit{G}}_{k-1}^T$$

(31)

Step 3: Update.

(1) State update.

Define the cost function $J\left(\mathit{x}\right)$ as

$$\begin{array}{cc}\hfill J\left(\mathit{x}\right)& ={\displaystyle \frac{1}{2}}{({\mathit{y}}_k-{\mathit{H}}_k\mathit{x})}^T{\mathit{R}}_k^{-1}({\mathit{y}}_k-{\mathit{H}}_k\mathit{x})\hfill \\ \hfill & +{\displaystyle \frac{1}{2}}{(\mathit{x}-{\widehat{\mathit{x}}}_k^{-})}^T{\left({\mathit{P}}_k^{-}\right)}^{-1}(\mathit{x}-{\widehat{\mathit{x}}}_k^{-})\hfill \end{array}$$

(32)

then the object is to find ${\widehat{\mathit{x}}}_k=arg\underset{\mathit{x}}{min}J\left(\mathit{x}\right)$ and $\dot{J}\left(\mathit{x}\right)\ge 0$.

In order to use the conjugate gradient method, given the following transformation: ${\mathit{x}}_k-{\widehat{\mathit{x}}}_k^{-}={\mathit{M}}_k{\mathit{z}}_k$, where ${\mathit{M}}_k$ is determined by the one-step prediction covariance matrix ${\mathit{P}}_k^{-}$ by ${\mathit{M}}_k{\mathit{M}}_k^T={\mathit{P}}_k^{-}$.

By the transformation, cost function can be revised as

$$\begin{array}{cc}\hfill J\left({\mathit{z}}_k\right)& ={\displaystyle \frac{1}{2}}{\mathit{z}}_k^T{\mathit{z}}_k+{\displaystyle \frac{1}{2}}{({\mathrm{Y}}_k-{\mathit{H}}_k{\mathit{M}}_k{\mathit{z}}_k)}^T{\mathit{R}}_k^{-1}({\mathrm{Y}}_k-{\mathit{H}}_k{\mathit{M}}_k{\mathit{z}}_k)\hfill \end{array}$$

(33)

where ${\mathrm{Y}}_k={\mathit{y}}_k-{\mathit{H}}_k{\widehat{\mathit{x}}}_k^{-}$, then it can be easily checked that

$$\begin{array}{cc}\hfill \nabla J\left({\mathit{z}}_k\right)& ={\mathit{z}}_k-{\mathit{M}}_k^T{\mathit{H}}_k^T{\mathit{R}}_k^{-1}({\mathrm{Y}}_k-{\mathit{H}}_k{\mathit{M}}_k{\mathit{z}}_k)\hfill \end{array}$$

(34)

and

$$\begin{array}{cc}\hfill {\nabla }^{2}J\left({\mathit{z}}_k\right)& =\mathit{I}+{\mathit{M}}_k^T{\mathit{H}}_k^T{\mathit{R}}_k^{-1}{\mathit{H}}_k{\mathit{M}}_k\hfill \end{array}$$

(35)

For the application of DPOS, ${\mathit{R}}_k$ denotes the measurement noise covariance which is decided by sensors noise level and then ${\mathit{R}}_k$ is a diagonal matrix with poitive elements, then the second order derivative ${\nabla }^{2}J\left({\mathit{z}}_k\right)$ is positive definite. In addition, ${\mathit{x}}_k$ can be obtained from the optimization process of ${\mathit{z}}_k$ by conjugate gradient method and ${\mathit{x}}_k={\widehat{\mathit{x}}}_k^{-}+{\mathit{M}}_k{\mathit{z}}_k$.

The specific optimization process ${\mathit{z}}_k$ is given as:

1) Initialize ${\mathit{r}}_1=\nabla J\left({\mathit{z}}_1\right)$, ${\mathit{P}}_1={\mathit{r}}_1$, ${\mathit{z}}_1=\mathbf{0}$, $m=1$;

2) Calculate ${\mathbf{\Gamma }}_m={\nabla }^{2}J\left({\mathit{z}}_m\right)$, ${\mathit{\alpha }}_m={\displaystyle \frac{-{\mathit{r}}_m^T{\mathit{p}}_m}{{\mathit{p}}_m^T{\mathbf{\Gamma }}_m{\mathit{p}}_m}}$;

3) ${\mathit{z}}_{m+1}={\mathit{z}}_m+{\mathit{\alpha }}_m{\mathit{p}}_m$;

4) ${\mathit{r}}_{m+1}=\nabla J\left({\mathit{z}}_{m+1}\right)$;

5) ${\mathit{\beta }}_{m+1}={\displaystyle \frac{{\mathit{r}}_{m+1}^T({\mathit{r}}_{m+1}-{\mathit{r}}_m)}{{\mathit{r}}_m^T{\mathit{r}}_m}}$;

6) ${\mathit{p}}_{m+1}=-{\mathit{r}}_{m+1}+{\mathit{\beta }}_{m+1}{\mathit{p}}_m$;

Then by 1)-6), one can use the conjugate gradient method to obtain the estimation of ${\mathit{z}}_k$ recursively, and then ${\mathit{x}}_k$ can be calculated as the state estimation.

(2) Covariance matrix update.

One can use the measurement linearity and formula (28) to calculation the error covariance matrix update.

By the above descriptions, the proposed central difference variational filtering algorithm has been presented, the detailed algorithm flow can be showed in Algorithm 2.

Algorithm 2:Central difference variational filtering algorithm

Initialization: ${\widehat{\mathit{x}}}_0,{\mathit{P}}_0$

Prediction

For $k=1$ to N do

1: ${\mathit{P}}_{k-1}={\mathit{S}}_{k-1}{\mathit{S}}_{k-1}^T$

2: ${\mathit{\chi }}_{0,k-1}={\widehat{\mathit{x}}}_{k-1},{\mathit{\chi }}_{l,k-1}={\widehat{\mathit{x}}}_{k-1}+\upsilon {\left({\mathit{S}}_{k-1}\right)}_l,{\mathit{\chi }}_{l+n,k-1}={\widehat{\mathit{x}}}_{k-1}-\upsilon {\left({\mathit{S}}_{k-1}\right)}_l$, $i=0,1,...,2n,l=1,2,...,n$, $\upsilon =\sqrt{3}$

3: ${\mathit{\chi }}_{i,k}^{-}=\mathit{f}\left({\mathit{\chi }}_{k-1}^i\right)$

4: ${\widehat{\mathit{x}}}_k^{-}=\sum _{i=0}^{2n}{\omega }_i^0{\mathit{\chi }}_{i,k}^{-}$

5: ${\tilde{\mathit{S}}}_k^{-}=[\sqrt{{\omega }_1^1}({\mathit{\chi }}_{1,k}^{-}-{\mathit{\chi }}_{1+n,k}^{-})\sqrt{{\omega }_2^1}({\mathit{\chi }}_{2,k}^{-}-{\mathit{\chi }}_{2+n,k}^{-})...\sqrt{{\omega }_n^1}({\mathit{\chi }}_{n,k}^{-}-{\mathit{\chi }}_{2n,k}^{-})\sqrt{{\omega }_1^2}({\mathit{\chi }}_{1,k}^{-}+{\mathit{\chi }}_{1+n,k}^{-}-2{\mathit{\chi }}_{0,k}^{-})\sqrt{{\omega }_2^2}({\mathit{\chi }}_{2,k}^{-}+$

${\mathit{\chi }}_{2+n,k}^{-}-2{\mathit{\chi }}_{0,k}^{-})...\sqrt{{\omega }_n^2}({\mathit{\chi }}_{n,k}^{-}+{\mathit{\chi }}_{2n,k}^{-}-2{\mathit{\chi }}_{0,k}^{-})]$

6: ${\mathit{P}}_k^{-}={\tilde{\mathit{S}}}_k^{-}{\left({\tilde{\mathit{S}}}_k^{-}\right)}^{\mathit{T}}+{\mathit{G}}_{k-1}{\mathit{Q}}_{k-1}{\mathit{G}}_{k-1}^T$

end

Update

Initialization: ${\mathit{r}}_1=\nabla J\left({\mathit{z}}_1\right)$, ${\mathit{p}}_1={\mathit{r}}_1$, ${\mathit{z}}_1=\mathbf{0}$, $m=1$

For $m=1,2,...,\mathrm{M}$ do

7. ${\mathit{\alpha }}_m={\displaystyle \frac{-{\mathit{R}}_m^T{\mathit{p}}_m}{{\mathit{p}}_m^T\mathit{A}{\mathit{p}}_m}}$

8. ${\mathbf{\Gamma }}_m={\nabla }^{2}J\left({\mathit{z}}_m\right)$, ${\mathit{\alpha }}_m={\displaystyle \frac{-{\mathit{r}}_m^T{\mathit{p}}_m}{{\mathit{p}}_m^T{\mathbf{\Gamma }}_m{\mathit{P}}_m}}$

9. ${\mathit{z}}_{m+1}={\mathit{z}}_m+{\mathit{\alpha }}_m{\mathit{p}}_m$

10. ${\mathit{r}}_{m+1}=\nabla J\left({\mathit{z}}_{m+1}\right)$

11. ${\mathit{\beta }}_{m+1}={\displaystyle \frac{{\mathit{R}}_{m+1}^T({\mathit{R}}_{m+1}-{\mathit{R}}_m)}{{\mathit{R}}_m^T{\mathit{R}}_m}}$

11. ${\mathit{p}}_{m+1}=-{\mathit{R}}_{m+1}+{\mathit{\beta }}_{m+1}{\mathit{p}}_m$

12. $m=m+1$

13: ${\mathit{x}}_k={\mathit{x}}_k^{-}+{\mathit{M}}_k{\mathit{z}}_{\mathrm{M}}$

14: ${\mathit{P}}_k={\mathit{P}}_k^{-}-{\mathit{P}}_k^{-}{\mathit{H}}_k^T{({\mathit{H}}_k{\mathit{P}}_k^{-}{\mathit{H}}_k^T+{\mathit{R}}_k)}^{-1}{\mathit{H}}_k{\mathit{P}}_k^{-}$

end

Return ${\widehat{\mathit{x}}}_k$, ${\mathit{P}}_k$

4. Experiment Validation

This section we apply the proposed algorithm to the master and slave system information fusion of DPOS. Specially, use the master system motion parameters and lever arm compensation as the external measurement of slave system, then by the proposed algorithm to estimate errors and correct the strapdown solution of the slave system. Detailed slave system error model is depicted in this section firstly, and then flight test and experimental results compared with the existing central difference filtering algorithm are given.

4.1. State-Space Model

Generally, level east-north-up frame is selected as navigation frame (n frame), $i,p,b,e$ denote the inertial frame, platform frame, slave system body frame, the earth-fixed frame, respectively. To present the state-space description of the slave system, we firstly list the error model of position, velocity, attitude and inertial sensors of system briefly [7], as shown below.

The fundamental inertial error model description is as follows.

$$\begin{array}{cc}\hfill & {\dot{\mathit{\varphi }}}^n=(\mathit{I}-{\mathit{C}}_n^p){\mathit{\omega }}_{in}^n+\delta {\mathit{\omega }}_{in}^n-{\mathit{C}}_b^n{\epsilon }^b-{\mathit{C}}_b^n{\mathit{w}}_{\epsilon }^b\hfill \\ \hfill & \delta {\dot{\mathit{V}}}^n=(\mathit{I}-{\mathit{C}}_p^n){\mathit{f}}^n-(2\delta {\mathit{\omega }}_{ie}^n+\delta {\mathit{\omega }}_{en}^n)\times {\mathit{V}}^n\hfill \\ \hfill & -(2{\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n)\times \delta {\mathit{V}}^n+{\mathit{C}}_b^n{\nabla }^b+{\mathit{C}}_b^n{\mathit{w}}_{\nabla }^b\hfill \\ \hfill & \delta \dot{L}=-{\displaystyle \frac{V_N\delta h}{{(R_M+h)}^2}}+{\displaystyle \frac{\delta V_N}{R_M+h}}\hfill \\ \hfill & \delta \dot{\lambda }={\displaystyle \frac{V_{E}secLtanL\delta L}{R_N+h}}-{\displaystyle \frac{V_{E}secL\delta h}{{(R_N+h)}^2}}+{\displaystyle \frac{secL\delta V_E}{R_N+h}}\hfill \\ \hfill & \delta \dot{h}=\delta V_U\hfill \\ \hfill & {\dot{\epsilon }}^b=\mathit{0}\hfill \\ \hfill & {\dot{\nabla }}^b=\mathit{0}\hfill \end{array}$$

(36)

where ${\mathit{\varphi }}^n={\left[{\varphi }_E{\varphi }_N{\varphi }_U\right]}^T$ denotes the attitude angle error, the velocity error is $\delta {\mathit{V}}^n$, $\delta L$, $\delta \lambda$ and $\delta h$ are the errors of latitude, longitude and altitude, respectively. ${\epsilon }^b$ denotes the gyroscopes constant drift vector in b frame and $w_{\epsilon }^b$ is the gyroscopes random noises. ${\nabla }^b$ is the accelerometers constant bias and $w_{\nabla }^b$ is the random noises of accelerometers. The symbols ${\omega }_{in}^n$, ${\omega }_{ie}^n$, ${\omega }_{en}^n$ can be found in detail in related literatures, and $\delta {\omega }_{in}^n$, $\delta {\omega }_{ie}^n$, $\delta {\omega }_{en}^n$ are the errors, respectively. ${\mathit{f}}^n$ is the specific forces measured by the accelerometers. ${\mathit{C}}_b^n$ and ${\mathit{C}}_n^p$ are the direction cosine matrices between n frame and b frame, p frame and n frame, respectively.

Also, consider the misalignment angle between the master and slave system as constant, that is, ${\dot{\mu }}_x={\dot{\mu }}_y={\dot{\mu }}_z=0$, and the flexible lever arm effect can be described by the Markov model as follows.

$$\begin{array}{cc}\hfill & {\ddot{\zeta }}_x+2{\eta }_x{\dot{\zeta }}_x+{\eta }_x^2{\zeta }_x={\xi }_x\hfill \\ \hfill & {\ddot{\zeta }}_y+2{\eta }_y{\dot{\zeta }}_y+{\eta }_y^2{\zeta }_y={\xi }_y\hfill \\ \hfill & {\ddot{\zeta }}_z+2{\eta }_z{\dot{\zeta }}_z+{\eta }_z^2{\zeta }_z={\xi }_z\hfill \end{array}$$

(37)

where ${\zeta }_i,i=x,y,z$ is the flexure angle, ${\eta }_i=2.146/{\tau }_i$ is the parameter, ${\tau }_i$ is the correlation, ${\xi }_i$ is the noise vector.

Differences of velocity and attitude between the master and slave system are used as the measurement

$$\mathit{y}={\left[\delta \varphi \delta \theta \delta \gamma \delta V_E^{{ }^{\prime }}\delta V_N^{{ }^{\prime }}\delta V_U^{{ }^{\prime }}\right]}^T$$

(38)

then state space model can be given as

$$\begin{array}{cc}\hfill & \dot{\mathit{x}}=\mathit{f}\left(\mathit{x}\right)+\mathit{G}\mathit{w}\hfill \\ \hfill & \mathit{y}=\mathit{H}\mathit{x}+\mathit{V}\hfill \end{array}$$

(39)

where $\mathit{x}=[{\varphi }_E{\varphi }_N{\varphi }_U\delta V_E\delta V_N\delta V_U\delta L\delta \lambda \delta h$${\epsilon }_x{\epsilon }_y{\epsilon }_z{\nabla }_x{\nabla }_y{\nabla }_z{\mu }_x{\mu }_y{\mu }_z{\zeta }_x{\zeta }_y{\zeta }_z{\dot{\zeta }}_x$ ${\dot{\zeta }}_y{\dot{\zeta }}_z{]}^T$ and the nonlinear function $\mathit{f}$ is decided by (36)-(37). $\mathit{w}$ and $\mathit{v}$ are the process noise vector with zero mean, variance $\mathit{Q}$ and measurement noise with zero mean, variance $\mathit{R}$, respectively.

Matrices $\mathit{G}$ and $\mathit{H}$ are

$$\begin{array}{c}\mathit{G}=\left[\begin{array}{cc}{\mathit{C}}_b^n& {\mathit{0}}_{3\times 3}\\ {\mathit{0}}_{3\times 3}& {\mathit{C}}_b^n\\ {\mathit{0}}_{9\times 3}& {\mathit{0}}_{9\times 3}\end{array}\right]\end{array}$$

(40)

$$\begin{array}{cc}\hfill & \mathit{H}=\left[\begin{array}{cccccc}{\mathit{H}}_1& {\mathit{0}}_{3\times 3}& {\mathit{0}}_{3\times 9}& {\mathit{H}}_2& {\mathit{H}}_3& {\mathit{0}}_{3\times 3}\\ {\mathit{0}}_{3\times 3}& {\mathit{I}}_{3\times 3}& {\mathit{0}}_{3\times 9}& {\mathit{0}}_{3\times 3}& {\mathit{0}}_{3\times 3}& {\mathit{0}}_{3\times 3}\end{array}\right]\hfill \end{array}$$

(41)

where the elements of $\mathit{H}$ are dicided by the cosine direction matrix of the master system, and for brevity, omitted here.

4.2. Experiment Process and Result Analysis

DPOS flight test is arranged for the algorithm validation, where the experiment equipment contains the flight plane, DPOS system, distributed SAR antenna for imaging, shown in Figure 1. In detail, the master system includes a high accuracy fiber gyroscope IMU and a GPS receiver, the slave system installs only same IMU, and the master and slave system locates near the SAR antenna, respectively. Distributed data processing computer is installed in the cabin to complete data processing and storage for the master and slave systems, then send motion parameters information to the imaging antenna. In addition, the rovering GPS antenna is fixed on the top of the plane, the GPS base station is in a clearing below the imaging area and data is collected for post-processing. The whole flight test lasts 4 hours, and the trajectory is shown in Figure 2, where the red line denotes the imaging area.

Figure 1. Flight airplane.

Figure 1. Flight airplane.

Figure 2. Flight experiment trajectory.

Figure 2. Flight experiment trajectory.

Table 1. Performance index of the experiment devices.

Items	Index
Gyrosocpe constant drift	0.01° /h
Accelerometer constant bias	20$\mu$g

Table 1. Performance index of the experiment devices.

Items	Index
Gyrosocpe constant drift	0.01° /h
Accelerometer constant bias	20$\mu$g

After the data acquisition, we firstly fuse the GPS information and the master system to obtain the external measurement of the slave system. Then we conduct noise adding on the slave system data to lower the inertial sensors accuracy to 0.1${ }^{\circ }$/h and 100$\mu$g, respectively. And the noise adding is determined according to the real inertial sensors noise characteristic. Next establishing the Markov model on the calibrated initial lever arm between the master and slave system $r_0=[1.52143.4452-0.0851]$ (m) to describe the flexure lever arm among the imaging area. Finally, after completing the above process, the proposed algorithm and existing Central Difference Kalman filtering algorithm in literature [20] are used to fuse the master and slave system information, motion parameters of slave system is obtained. The method in literature [20] is also concentrated on improvement of the filtering update process, which adopts the $H_{\infty }$ filtering algorithm to suppress the disturbances on the filtering accuracy. Our proposed method is also served for this, which improves the filtering process by using the conjugate gradient method to substitute the linear minimum variance estimation. Next the comparison between these methods is showed in detail.

By processing the experiment data, the master system motion parameters information and lever arm transformation as the reference of slave system, the error curves of the slave system in dashed line are shown in Figure 3, Figure 4 and Figure 5, respectively. Since the DPOS is served for imaging task, then we only concentrate on the imaging area. Also, the imaging task is composed of several laps around imaging area, every lap lasts for about 600s, then 600s data result is presented to show the superiority of the proposed algorithm.

Figure 3 presents the attitude estimation comparison,including the heading error, pitch error and roll error comparison of the above three methods. In Figure 3, the proposed method seems easier to converge than the traditional method and the method in [20] from the curve. In the filtering prediction stage, compared with the traditional linear minimum variance estimation update and $H_{\infty }$ filtering update in [20], the proposed method uses the central difference kalman filtering interpolation method and conjugate gradient iteration update to substitute derivative computation to propagate the system characteristic, to some extent, the CDVF can suffer adverse effects by big maneuvers like turns.

Figure 4 shows the accuracy of the proposed approach is higher and more stable than that of the traditional method in terms of the east velocity error, the north velocity error and up velocity error. The proposed method uses variational optimization by conjugate gradient method to conduct the state update instead of the linear minimum variance estimation; Meanwhile, the state estimation covariance matrix update is to be in progress parallel with the state update by the original central difference kalman filtering and it could in theory be more accurate since they capture more estimation than the linear minimum variance and $H_{\infty }$ filtering update process in [20].

Figure 5 indicates the accuracy of the position is significantly improved via the proposed approach compared to the traditional method and $H_{\infty }$ filtering update process in [20]. From the Figure 5, the stability of the proposed method is stronger than that of the existing two methods in terms of latitude, longitude and height. From Figure 4 and Figure 5, on one hand, the east velocity error is in accordance with the longitude error in Figure 5; on the other hand, the latitude error in Figure 5 varies as the north velocity error varies, which meets the error propagation law of inertial systems.

What’s more, a table of their accuracy statistics is presented in a detailed quantization form. From the table, the accuracy of the proposed approach is almost all higher than that of the traditional method and $H_{\infty }$ filtering update process in [20], which is in accordance with Figure 2, Figure 3 and Figure 4 in above. In terms of the heading error, pitch error and roll error, their improvement is 22.2%,10.5% and 12.5%, respectively. The velocity errors of the two approaches about east velocity error, north velocity and up velocity are reduced by 19.2%, 18.1% and 22.5%, respectively. In addition, the latitude error, longtitude error and height error are reduced by 17.3%, 19.8% and 23.9%, respectively. In the terms of algorithm complexity, the proposed algorithm is slower than the other algorithms, but it is almost unaffected for the data processing computer. In a word, the estimation performance of the motion parameter is improved using CDVF based on conjugate gradient method.

In two-dimensional images, due to the presence of overlapping masking, multiple targets of different heights fall into the same distance and azimuth unit, making it difficult to achieve target reconstruction and recognition. As shown in Figure 6, overlapping phenomena have occurred between buildings and the ground, as well as between buildings, making it difficult to determine information such as the number, structure, size, and location of buildings.

The motion estimation of the proposed method is used to compensate array SAR with three-dimensional imaging capability, which can achieve three-dimensional imaging of complex targets such as buildings. Using ADPOS data for motion compensation of 2D imaging of multi-channel array SAR, achieving multi-channel consistency compensation through image registration and amplitude phase correction, then focusing in high dimensions, modeling buildings, and pasting scattering information on the model to obtain 3D imaging results, as shown in the Figure 7. It can be seen that the 3D imaging results clearly reflect the structural information of the building and the texture information of the side and roof, greatly reducing the difficulty of SAR image interpretation and verifying the effectiveness of the method proposed in this paper.

5. Conclusion and Future Work

In this work, a central difference variational filtering algorithm is developed to serve for the master and slave system information fusion of ADPOS. Compared with the general state estimation methods which is established on the LMV criterion approximately, this work takes full use of the advantages of variational optimization in the filtering update stage and then central difference variational filtering algorithm is proposed, applied to the ADPOS filght test, the experiment results show the proposed algorithm has a distinct improvement in the multinode motion parameters accuracy, which can be used in array imaging.

In the future, more efficient filtering algorithm will be considered and low cost slave systems will be used in the flight test, which optimize the filtering process by better optimization methods, also real-time performance will be considered to advance the development of areial distributed imaging application.