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

Abstract

The airborne distributed position and orientation system (ADPOS), which integrates multi-inertia measurement units (IMUs), a data-processing computer, and a Global Navigation Satellite System (GNSS), serves as a key sensor in new higher-resolution airborne remote sensing applications, such as array SAR and multi-node imaging loads. ADPOS can provide reliable, high-precision and high-frequency spatio-temporal reference information to realize multinode motion compensation with the various nonlinear filter estimation methods such as Central Difference Kalman Filtering (CDKF), and modified CDKF. Although these known nonlinear models demonstrate good performance, their noise estimation performance with its linear minimum variance estimation criterion is limited for ADPOS. For this reason, in this paper, Central Difference Variational Filtering (CDVF) based on the variational optimization process is presented. This method adopts the conjugate gradient algorithm to enhance the estimation performance for mean correction in the filtering update stage. On one hand, the proposed method achieves adaptability by estimating noise covariance through the variational optimization method. On the other hand, robustness is implemented under the minimum variance estimation criterion based on the conjugate gradient algorithm to suppress measurement noise. We conducted a real ADPOS flight test, and the experimental results show that the accuracy of the slave motion parameters has significantly improved compared to the current CDKF. Moreover, the compensation performance shows a clear enhancement.

1. Introduction

With the development of the airborne remote sensing system, its higher imaging resolution and three-dimensional imaging is increasingly arousing more interest in new research fields, such as interferometric SAR (InSAR) and array antenna SAR imaging. In both of these new fields, multiantennas are positioned at different locations [1]. Correspondingly, the motion compensation equipment is developed from a single POS to multiple in different locations with the assistance of ADPOS [2,3,4,5]. In terms of the ADPOS system architecture, an integrated system with a high-precision inertial measurement unit (IMU) and GNSS [6] serves as the master POS, which combines strapdown inertial solutions with state correction to fuse the inertial and satellite sensor information using a linear model. Several low-cost slave systems rely on fusing the master system’s information with their own data to improve parameter accuracy using various state estimation algorithms. The complete system provides multi-node, high-frequency, and high-accuracy motion parameters to multi-node loads for higher-resolution three-dimensional imaging [7].

In order to compensate for motion errors in multinode loads, ADPOS has to address the deterministic errors by calibration and stochastic errors using recursive state estimation. When the inertial sensors for ADPOS are determined, stochastic errors are more important than determined errors in performance improvement. Therefore, filter estimation algorithms play a vital role in the transfer alignment of DPOS, which has a direct impact on the recursive state estimation performance. The transfer model of the ADPOS is inherently nonlinear. However, earlier methods such as the widely-used KF are subject to the divergence that results from approximations during any linearization process [8,9]. As a result, the EKF becomes the more widely-applied estimator for the nonlinear model, using a first-order linearization method based on a suboptimal recursive estimation framework. However, EKF has some fatal shortcomings, which dramatically degrade the accuracy and cause divergence of the state estimation when the nonlinear characteristic is very strong [10].

In order to overcome the shortcomings of the aforementioned traditional state estimation algorithm, a series of nonlinear filtering algorithms have been deduced to further develop nonlinear state estimation in the past years [9]. In addition to these algorithms, several nonlinear state estimation methods have been generated from deterministic sampling methods or numerical methods. And the linear minimum variance criterion is applied to the filtering update process [11], as occurs in the widely-used unsecnted Kalman filter [12] and the central difference Kalman filter [13]. In light of this, the algorithm flow is obtained using the linear minimum variance criterion approximately. However, this is not the actual case, and therefore more efficient filtering update methods that integrate the Kalman filter framework need to be considered. Nonlinear state estimation algorithms based on deterministic sampling using a new probabilistic framework have been developed, such as the unscented Kalman filter (UKF) and the central difference Kalman filter (CDKF). Both of these methods rely on approximating the probability density distribution of the nonlinear function [14]. Considering these advantages, such state estimation algorithms avoid the need for Jacobian matrix calculations and truncation errors, making them well-suited for handling nonlinear estimation problems [1]. Thus, higher estimation accuracy is achieved. Also, compared with the UKF algorithm, the CDKF algorithm requires fewer parameters to generate sigma points for nonlinear estimation while maintaining the same level of accuracy, making it one of the most widely used methods in nonlinear estimation [15]. However, since the transfer alignment model is high-dimensional, CDKF and modified CDKF may not meet the required estimation accuracy. Addressing how to enhance CDKF performance for high-dimensional models is crucial.

For the filtering update process, one can treat it as an unconstrained optimization problem to be solved. Among these fields, the steepest descent algorithm (gradient descent algorithm), the Newton method and the related Quasi-Newton method, and the conjugate gradient algorithm are widely used [16,17]. The conjugate gradient algorithm lies between the steepest descent algorithm and Newton method. It only employs the first order derivative information, overcoming the defect of slow convergence of steepest descent algorithm. In addition, it is immune to the calculation and storage of the Hessian matrix. Because of its simple structure, smaller storage capacity, and high numerical efficiency, the conjugate gradient method has been one of the most efficient methods to solve the nonlinear optimization problem and is used in many fields. Developed from Hestenes and Stiefel in the 1950s, conjugate gradient methods have been studied by many researchers [18]. There are many derivatives developed from these basic methods, such as the Hestenes–Steifel (HS) method [19], the Fletcher–Reeves (FR) method [20], the Polak–Ribiére-Polyak (PRP) method [21], the conjugate descent (CD) method [22], the Liu–Storey (LS) method [23], and the Dai–Yuan (DY) method [24]. These methods differ from others in their approach to selecting step length factors. Specifically, the FR, CD, and DY methods perform well in convergence performance, but poorly in numerical performance. While the PRP, LS, and HS methods can self-correct when encountering small step sizes continuously, showing better numerical performance, we focus on the PRP method for conducting the conjugate gradient to achieve optimization.

Compared with general filtering estimation methods, the related derivatives using the linear minimum variance criterion assume that the state estimation is a linear regression with the observation [25]. Variational optimization is an effective method to replace the Kalman linear regression with quadratic terms in the observation to handle nonlinearities [26,27]. It combines the conjugate gradient method with variational optimization to estimate state mean, 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 the state update. Motivated by the above description, based on the superiority of variational optimization with the conjugate gradient method, CDKF adopts fewer parameters to achieve estimation, and a central difference-based variational filtering algorithm (CDVF) is proposed to contribute to the transform alignment 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 CDKF interpolation method to substitute derivative computation to propagate the system characteristic. (2) For the filtering update stage, the CDVF algorithm uses variational optimization with conjugate gradient method to conduct the state update instead of the linear minimum variance estimation. Meanwhile, the state estimation covariance matrix update progresses in parallel with the state update by the original CDKF.It can obviously improve the accuracy of the slave system motion parameters. (3) A real flight test is conducted to verify the proposed algorithm. Detailed experiment results are presented and show the superiority of the proposed algorithm.

The outline of the remainder of this work is arranged as follows: In Section 2, the background theories, including the linear minimum variance criterion, the central difference Kalman filtering algorithm, and the conjugate gradient optimization method, are listed successively. Compared with the linear minimum variance criterion based on CDKF algorithm, combining the central difference interpolation method and the variational optimization method with the conjugate gradient method, the central difference variational filtering algorithm is proposed in Section 3. The detailed experiment validation process, including experiment design, data processing, and analysis of the results, is located in Section 4. Finally, the conclusion and future work are displayed in Section 5.

2. Related Theories

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

The nonlinear system with additive noise can be represented 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 [25]

Let $\mathit{x}$ be the variable to be estimated where $\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}$. This can also be denoted as $E^{*}[\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)

Here, 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 such 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^{*}[{\mathit{x}}_k/{\mathit{y}}_{1:k}]=E^{*}[{\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^{*}[{\mathit{x}}_k/{\mathit{y}}_{1:k-1}]+E^{*}[{\mathit{x}}_k/{\mathit{y}}_k]-E\left[{\mathit{x}}_k\right]\hfill \\ \hfill & ={\widehat{\mathit{x}}}_k^{-}+E^{*}[{\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^{*}[{\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^{*}[{\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. Thus, 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, applying the linear minimum variance criterion and the Gaussian weighted integrals to the nonlinear filtering equation, the Gaussian filtering is obtained [11]. Especially, Stirling’s interpolation approximation generates the central difference Kalman filtering and is used to solve the numerical integrals to complete the recursive estimation [13].

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,\dots ,2n,l=1,2,\dots ,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 and 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)

Considering 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 simplify the filtering covariance matrix update. Then it can progress in parallel with the state update using the one-step prediction ${\mathit{P}}_k^{-}$, measurement matrix ${\mathit{H}}_k$, and 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. 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.

The conjugate gradient method is very efficient for solving large-scale unconstrained optimization problems. A 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 the conjugate gradient method (Algorithm 1). 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 in the selection of $\mathit{\beta }$. Here, we list the algorithm flow of the PRP conjugate gradient method as following [21].

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 is used to conduct the state prediction, covariance matrix prediction and update. Meanwhile, the conjugate gradient method is used to update the state instead of the LMV criterion. Combining the algorithm flow of the central difference Kalman filtering and the conjugate gradient method, we present the outline of the central difference variational filtering algorithm as follows [26].

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)

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, the following transformation is required: ${\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, the 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 be seen 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 ADPOS, ${\mathit{R}}_k$ denotes the measurement noise covariance, which is decided by the noise level of the sensors. Then ${\mathit{R}}_k$ is a diagonal matrix with positive elements, and 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 the 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 follows:

(1)

Initialize ${\mathit{r}}_1=\nabla J\left({\mathit{z}}_1\right)$, ${\mathit{p}}_1={\mathit{r}}_1$, ${\mathit{z}}_1$ = 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 calculate the error covariance matrix update.

By the above descriptions, the proposed central difference variational filtering algorithm has been presented, the detailed algorithm flowchart is shown in Figure 1, and the pseudo code is described 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,\dots ,2n,l=1,2,\dots ,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}^{-})\dots$

$\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}^{-})\dots \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$ = 0, $m=1$

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

7. ${\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}}$

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

9. ${\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}}$

10. ${\mathit{p}}_{m+1}=-{\mathit{r}}_{m+1}+{\mathit{\beta }}_{m+1}{\mathit{p}}_m$

11. $m=m+1$

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

13: ${\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

In this section, we apply the proposed algorithm to the transform alignment between the master and slave systems of ADPOS. Specifically, we will make use of the master system motion parameters and lever arm compensation as the external measurement of the slave system. Then we apply the proposed algorithm to estimate errors and correct the strapdown solution of the slave systems. First, a detailed slave system error model is depicted in this section, and then the flight test and experimental results are compared with the existing CDKF and modified CDKF.

4.1. State-Space Model

In ADPOS, an east-north-up frame is selected as the navigation frame (n frame), $i,p,b,$ and e denote the inertial frame, platform frame, slave system body frame, and the earth-fixed frame, respectively. To present the state-space description of the slave systems, we first briefly list the error model of the position, velocity, attitude and inertial sensors of the system [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{\mathit{\epsilon }}^b-{\mathit{C}}_b^n{\mathit{w}}_{\mathit{\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{\mathbf{\nabla }}^b+{\mathit{C}}_b^n{\mathit{w}}_{\mathbf{\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{\mathit{\epsilon }}}^b=\mathsf{0}\hfill \\ \hfill & {\dot{\mathbf{\nabla }}}^b=\mathsf{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. ${\mathit{\epsilon }}^b$ denotes the gyroscopes constant drift vector in the b frame and $w_{\epsilon }^b$ is the gyroscopes random noises. ${\mathit{\nabla }}^b$ is the accelerometers constant bias and $w_{\mathit{\nabla }}^b$ is the random noises of the accelerometers. The symbols ${\mathit{\omega }}_{in}^n$, ${\mathit{\omega }}_{ie}^n$, ${\mathit{\omega }}_{en}^n$ can be found in detail in related literatures, and $\delta {\mathit{\omega }}_{in}^n$, $\delta {\mathit{\omega }}_{ie}^n$, $\delta {\mathit{\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 the n frame and the b frame, and the 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, and ${\xi }_i$ is the noise vector.

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

$$\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 the 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) and (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& {\mathsf{0}}_{3\times 3}\\ {\mathsf{0}}_{3\times 3}& {\mathit{C}}_b^n\\ {\mathsf{0}}_{9\times 3}& {\mathsf{0}}_{9\times 3}\end{array}\right]\end{array}$$

(40)

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

(41)

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

4.2. Experiment Process and Result Analysis

The ADPOS flight test is arranged in order to validate the proposed method’s performance. And the experimental equipment consists of the flight plane, ADPOS, and the array SAR for imaging, as shown in Figure 2. In detail, the master system includes a high-precision fiber IMU and a GPS receiver, and the accuracy of these same slave systems is lower than the master system. The master and the slave systems are installed near the array SAR, respectively. A distributed data processing computer is installed in the cabin to complete data processing and storage for the master and the slave systems, which then sends motion parameters information to the array SAR. In addition, the GPS is fixed on the top of the plane while the GPS base station is located at the center of the imaging area. In addition, to ensure the positioning accuracy, we use the RTK function of GPS, and the data from the GPS base station is collected for post-processing.Their performance index is shown in

Table 1. Performance index of the experiment devices.

Items	Index
Gyrosocpe constant drift	0.01 °/h
Accelerometer constant bias	20 $\mathsf{\mu }$g
IMU sampling rates	200 Hz
GPS positioning accuracy	5 m
difference GPS positioning accuracy	0.02 m

. The whole flight test lasts 4 h, and the trajectory is shown in Figure 3, where the red line denotes the imaging area.

After the data acquisition, we first 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 °/h and 100 $\mathsf{\mu }$g, respectively. And the noise adding is determined according to the real inertial sensors’ noise characteristics. Next, we establish 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 modified CDKF in [1] are used to fuse the master and slave system information, and the motion parameters of the slave system is obtained. The method in [1] 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 also serves this purpose, 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 illustrated in detail.

By processing the experimental data, the master system motion parameters information and lever arm transformation are used as the reference for the slave system. The error curves of the slave system, shown in dashed lines, are presented in Figure 4, Figure 5 and Figure 6, respectively. Since the DPOS is used for imaging tasks, we focus exclusively on the imaging area. Also, the imaging task is composed of several laps around imaging area. Every lap lasts for about 600 s, and the data that result from that 600 s period are presented to show the superiority of the proposed algorithm.

Figure 4 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 modified method in [1] from the curve. In the filtering prediction stage, compared with the traditional linear minimum variance estimation update and $H_{\infty }$ filtering update, the proposed method uses the central difference Kalman filtering interpolation method and the conjugate gradient iteration update to substitute derivative computation to propagate the system characteristics. To some extent, the CDVF can suffer adverse effects by big maneuvers like turns.

Figure 5 demonstrates that 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 the up velocity error. The proposed method uses variational optimization via the conjugate gradient method to conduct the state update instead of the linear minimum variance estimation; Meanwhile, the state estimation covariance matrix update progresses in 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 [1].

Figure 6 indicates that the accuracy of the position is significantly improved via the proposed approach compared to the traditional method and the $H_{\infty }$ filtering update process. From Figure 5, we see that the stability of the proposed method is stronger than that of the two existing methods in terms of latitude, longitude, and height. In Figure 4 and Figure 5, on the 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, thus fulfilling the error propagation law of inertial systems.

Moreover, in order to quantitatively evaluate the precision of the proposed method, the standard deviation (STD) of multi-node motion parameter errors is calculated three times and the maximum value is used as the final precision of the CVKF. The statistical results of the errors are shown in a detailed quantization form. In

Table 2. RMSE comparison between the algorithms.

	Items	The Traditional Method	The Modified Method	The Proposed Method
Attitude (°)	Heading	0.066	0.058	0.050
	Pitch	0.006	0.005	0.004
	Roll	0.005	0.004	0.003
Velocity (m/s)	East velocity	0.026	0.023	0.019
	North velocity	0.023	0.020	0.016
	Up velocity	0.031	0.026	0.022
Position (m)	Latitude	0.748	0.645	0.580
	Longitude	0.848	0.713	0.660
	Height	0.976	0.870	0.741

, we see that the accuracy of the proposed approach is all higher than that of the traditional method and the $H_{\infty }$ filtering update process, which is in accordance with Figure 4, Figure 5 and Figure 6 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 for 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. However, CDVF is higher than the other two methods in computational load and computational complexity, because the proposed method uses the multipoint iteration for optimization in the state update step. In a word, the estimation performance of the motion parameter is obviously improved using CDVF based on the 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 7, 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 the array SAR with three-dimensional imaging capability, enabling three-dimensional imaging of complex targets, such as buildings. ADPOS data are used for motion compensation of 2D imaging of the multi-channel array SAR, achieving multi-channel consistency compensation through image registration and amplitude phase correction. This is followed by high-dimensional focusing, building modeling, and pasting scattering information on the model to obtain 3D imaging results, as shown in the Figure 8. The 3D imaging results clearly reveal the structural details of the building, along with the texture information of the sides and roof, significantly reducing the difficulty of SAR image interpretation and validating the effectiveness of the method proposed in this paper.

5. Conclusions and Future Work

In this work, a central difference variational filtering algorithm is developed to facilitate the master and slave system information fusion of ADPOS. Compared with the general state estimation methods based on the approximate LMV criterion, this work makes full use of the advantages of variational optimization in the filtering update stage. The proposed central difference variational filtering algorithm is applied to the ADPOS filght test and the experimental results show that the proposed algorithm demonstrates a significant improvement in the accuracy of multinode motion parameters, which can be used in array imaging.

Since these methods are conducted in MATLAB, we will optimize the proposed method in a real-time system in the future. Moreover, a more efficient filtering algorithm will be considered and low-cost slave systems will be used in the flight test, which optimize the filtering process through better optimization methods. Also, real-time performance will be considered in order to advance the development of aerial distributed imaging applications.