# Closed-Form Distance Estimators under Kalman Filtering Framework with Application to Object Tracking

Authors: Vladimir Shin; Georgy Shevlyakov; Woohyun Jeong; Yoonsoo Kim
Journal: Mathematical Problems in Engineering
 (2020)

---

## Abstract

In this paper, the minimum mean square error (MMSE) estimation problem for calculation of distances between two signals via the Kalman filtering framework is considered. The developed algorithm includes two stages: the Kalman estimate of a state vector computed at the first stage is nonlinearly transformed at the second stage based on a distance function and the MMSE criterion. In general, the most challenging aspect of application of the distance estimator is calculation of the multivariate Gaussian integral. However, it can be successfully overcome for the specific metrics between two points in line, between point and line, between point and plane, and others. In these cases, the MMSE estimator is defined by an analytical closed-form expression. We derive the exact closed-form bilinear and quadratic MMSE estimators that can be effectively applied for calculation of an inner product, squared norm, and Euclidean distance. A novel low-complexity suboptimal estimator for special composite functions of linear, bilinear, and quadratic forms is proposed. Radar range-angle responses are described by the functions. The proposed estimators are validated through a series of experiments using real models and metrics. Experimental results show that the MMSE estimators outperform existing estimators that calculate distance and angle in nonoptimal manner.


---

## Body

## 1. Introduction

The problem of measuring the distance between real-valued signals or images arises in most areas of scientific research. In particular, the familiar Euclidean distance plays a prominent role in many important application contexts not only in engineering, economics, statistics, and decision theory, but also in fields such as machine learning, cryptography, image recognition, and others. The statistical methods related to the distance estimation can be categorized into image and signal processing areas.The concept of a distance metric is widely used in image processing and computer vision [1–5] (also see references therein). The distance provides a quantitative measure of the degree of match between two images or objects. These objects might be two profiles of persons, a person and a target profile, camera of a robot and people, or any two vectors taken across the same features (variables) derived from color, shape, and/or texture information. Image similarity measures play an important role in many image algorithms and applications including retrieval, classification, change detection, quality evaluation, and registration [6–12].The proposed paper deals with the distance estimation between random signals. In signal processing, a good distance metric helps in improving the performance of classification, clustering and localization in wireless sensor networks, radar tracking, and other applications [13–20]. The Bayesian classification approach based on concepts of the Euclidian and Mahalanobis distances is often used in discriminant analysis. Survey of the classification procedures which minimize a distance between raw signals and classes in multifeature space is given in [21, 22]. The distance estimation algorithm based on the goodness-of-fit functions where the best parameters of the fitting functions are calculated given the training data is considered in [23]. Algorithm for estimation of a walking distance using a wrist-mounted inertial measurement unit device is proposed in [24]. The concept of distance between two samples or between two variables is fundamental in statistics due to the fact that a sum of squares of independent normal random variables has a chi-square distribution. Knowledge of the distribution and usage of the usual approximations make a confidence interval for distance metrics [25, 26]. Usage of the Taylor series expansions for aircraft geometric-height estimation using range and bearing measurements is addressed in [27, 28]. The minimum mean square error (MMSE) estimation of a state vector in the presence of information about the absolute value of a difference between its subvectors is proposed in [29].In many applications, it is interesting to estimate not only a position or state of an object but also a nonlinear distance function which gives information to effectively control target tracking. However, most authors have not focused on a simultaneous estimation of a state and distance functions in dynamical models such as a Kalman filtering framework.The problem of estimation of the distance function,dk=dxk,yk, between two vector signals xk and yk is considered in the paper, but its difference from the aforementioned references is that the both signals xk and yk are unknown, and they should be simultaneously estimated with the function dk using indirect measurements. For example, we observe positions of two points Axk and Byk in a line and a distance between the points represents the absolute difference, i.e., dkA,B=xk−yk. The positions xk and yk and consequently the distance dk=xk−yk are unknown, and our problem is to optimally calculate three estimates x^k,y^k,andd^k. Note that the simple distance estimator d^k=x^k−y^k is not an optimal solution.The purpose of the paper is to derive an analytical closed-form MMSE estimator for distance functions between random signals such as the absolute value, the Euclidean distance, inner product, bilinear, and quadratic forms. The advantage of the estimator is quick and accurate calculation of distance metrics compared to the approximate or iterative estimators. A further study of using the estimators is also done for the object tracking problem where we can obtain important practical results for the distance estimation of signals in linear Gaussian discrete-time systems.The following list highlights the primary contributions of this paper:(1)
Extension of the MMSE approach to the estimation of a nonlinear functions of a state vector within the Kalman filtering framework. The obtained MMSE-optimal solution represents a two-stage estimator.(2)
Derivation of analytical expressions for the different metrics between two points in a line, between a point and a line, and between a point and a plane. We establish that the obtained estimators represent compact closed-form formulas depending on the Kalman filter state estimates and error covariance.(3)
The MMSE estimators for quadratic and bilinear forms of a state vector are investigated and applied, including the estimators for the square of a normxk22, the square of the Euclidean distance xk−yk22, and the inner product xk,yk. A novel low-complexity algorithm for suboptimal estimation of a special class of composite functions is proposed. Tracking radar responses such as range, angles, and range rate are described by the functions.(4)
Performance of the proposed MMSE estimators through real examples illustrates their theoretical and practical usefulness.This paper is organized as follows. Section2 presents a statement of the MMSE estimation problem for an arbitrary nonlinear function of a state vector within a Kalman filtering framework. In Section 3, the general MMSE estimator is proposed, and computational complexity of the estimator is discussed. The concept of a closed-form estimator is introduced. In Section 4, the closed-form MMSE estimator for absolute value of a linear form of a state vector is derived (Theorem 1). In particular cases, the estimator calculates distances between two points in 1-D line, between a point and line in 2-D plane, and between a point and plane in 3-D space. The comparative analysis of the estimator via several practical examples is presented. In Section 5, the MMSE estimators for quadratic and bilinear forms of a state vector are comprehensively studied (Theorems 2 and 3). Effective matrix formulas for the quadratic and bilinear MMSE estimators are derived and applied with the Euclidean distance, a norm, and inner product of vector signals. In Section 6, a low-complexity suboptimal estimator for composite nonlinear functions is proposed and recommended for calculation of radar range-angle responses. In Section 7, the efficiency of the suboptimal estimator is demonstrated on an 2-D dynamical model. Finally, we conclude the paper in Section 8. The list of main notations is given in Table 1.Table 1
List of main notations.
ℝnSet ofn-dimensional real column vectorsℝn×mSet ofn × m real matricesATTranspose of matrixAInIdentity matrix of sizen × nOnNull matrix of sizen × nA−1Inverse ofn × n matrix AtrATrace ofn × n matrix Aℕm,CNormal distribution with meanm and covariance matrix CE.Expectation operatorCovxkCovariance (covariance matrix) of random vectorxkCovxk,ykCross covariance of random vectorsxk and ykx2Euclidean norm (2-norm) of vector,x2=xTxx,yInner product of vectors,x,y=∑i=1nxiyi,x,y∈ℝncTxLinear form (LF),cTx=∑i=1ncixi,c,x∈ℝnxTAxQuadratic form (QF),xTAx=∑i,j=1naijxixj,A=aijxTBx˜Bilinear form (BLF),xTAx˜=∑i,j=1nbijxix˜j,B=bij


## 2. Problem Statement

The basic framework for the Kalman filter involves estimation of a state of a discrete-time linear dynamical system with additive Gaussian white noise:(1)xk+1=Fkxk+Gkvk,k=0,1,…,yk=Hkxk+wk,where xk∈ℝn is a state vector, yk∈ℝm is a measurement vector, and vk∈ℝr and wk∈ℝm are zero-mean Gaussian white noises with process Qk and measurement Rk noise covariances, respectively, i.e., vk∼ℕ0,Qk,wk∼ℕ0,Rk, and Fk∈ℝn×n, Gk∈ℝr×r, Qk∈ℝr×r, Rk∈ℝm×m, and Hk∈ℝm×n. The initial state x0∼ℕm0,C0 and the process and measurement noises vk,wk are mutually uncorrelated.In parallel with the state-space model (1), consider the nonlinear function of a state vector:(2)zk=fxk:ℝn⟶ℝ,which in particular case represents a distance metric in ℝn.Given the overall noisy measurementsyk=y1,y2,…,yk,k≥1, our goal is to desire optimal estimators x^k and z^k for the state vector (1) and nonlinear function (2), respectively.There are a multitude of statistics-based methods to estimate the unknown valuezk=fxk from the sensor measurements yk We focus on the MMSE approach, which minimizes the mean square error (MSE), minz^Ezk−z^k22, which is a common measure of estimator quality.The MMSE estimator is the conditional mean (expectation) of the unknownzk=fxk given the known observed value of the measurements, z^kopt=Ezkyk [30, 31]. The most challenging problem in the MMSE approach is how to calculate the conditional mean. In this paper, explicit formulas for distance metrics within the Kalman filtering framework are derived.


## 3. General Formula for Optimal Two-Stage MMSE Estimator

In this section, the optimal MMSE estimator for the general functionfxk of a state vector is proposed. It includes two stages: the optimal Kalman estimate of the state vector x^k computed at the first stage is used at the second stage for estimation of fxk.First stage (calculation of Kalman estimate): the mean square estimatex^k=Exkyk of the state xk based on the measurements yk and error covariance Pk=Covek,ek=xk−x^k are described by the recursive Kalman filter (KF) equations [30, 31]:(3)x^k+1−=Fkx^k,x^0=m0,k=0,1,…,Pk+1−=FkPkFkT+GkQkGkT,P0=C0,Kk+1=Pk+1−Hk+1THk+1Pk+1−Hk+1T+Rk+1−1,x^k+1=x^k+1−+Kk+1yk+1−Hk+1x^k+1−,Pk+1=In−Kk+1Hk+1Pk+1−,where x^k+1−=Exk+1yk and Pk+1−=Covek+1−,ek+1−=xk+1−x^k+1− are the time update estimate and error covariance, respectively, and Kk∈ℝn×m is the filter gain matrix.Second stage (optimal MMSE estimator): next, the optimal MMSE estimate of the nonlinear functionzk=fxk based on the measurements yk also represents a conditional mean, that is,(4)z^kopt=Ezkyk=∫ℝnfxpxykdx,where pxyk=ℕx^k,Pk is a multivariate conditional Gaussian probability density function.(5)ℕx^k,Pk=12πn/2Pk1/2exp−12x−x^kTPk−1x−x^k.Thus, the best estimate in equation (4) represents the optimal MMSE estimator, z^kopt=Fx^k,Pk, which depends on the Kalman estimate x^k and error covariance Pk determined by KF equation (3).Remark 1.
(closed-form MMSE estimator). In general case, the calculation of the optimal estimate,z^kopt=Ezkyk, is reduced to calculation of the multivariate Gaussian integral (5). The lack of the estimate is impossibility to calculate the integral in explicit form for the arbitrary nonlinear function fx. Analytical calculation of the integral (closed-form MMSE estimator) is possible only in special cases considered in the paper. The closed-form estimators for distance metrics in terms of x^k and Pk are proposed in Sections 4 and 5.
The Euclidean distance between two points,x1,x2∈ℝn, is defined as(6)dx1,x2=defx1−x22=∑i=1nx1,i−x2,i2.
In this particular case wherex1 and x2 represent two points located on the 1-D line, the Euclidean distance represents the absolute value (see Figure 1), i.e.,(7)dx1,x2=x1−x2.
In Section4, the MMSE estimator for the absolute value is comprehensively studied.Figure 1
Distancedx1,x2=x1−x2 between two moving points in 1-D line.


## 4. Closed-Form MMSE Estimator for Absolute Value

### 4.1. MMSE Estimator for Absolute Value of Linear Form

Lemma 1.
(MMSE estimator for |x|). Let x∈ℝ be a normal random variable, and x^ and P=Ex−x^2 are the MMSE estimate and error variance, respectively. Then, the MMSE estimator for the absolute value z=x has the following closed-form expression:(8)z^opt=Exyk=2Pπexp−x^22P+x^1−2Φ−x^P,where Φ⋅ is the cumulative distribution function of the standard normal distribution, ℕ0,1.The derivation of equation (8) is given in the Appendix.Letℓ=cTx+d be a linear form (LF) of the normal random vector, x∈ℝn, and x^∈ℝn and P∈ℝn×n are the MMSE estimate and error covariance, respectively. Then, the MMSE estimate of the linear form ℓ and its error variance can be calculated as(9)ℓ^=Eℓyk=cTx^+d,Pℓ=Eℓ−ℓ^2=cTPc,and we have the following theorem.Theorem 1.
(MMSE estimator for absolute value of LF). Letx∈ℝn be a normal random vector, and x^∈ℝn and P∈ℝn×n are the MMSE estimate and error covariance, respectively. Then, the closed-form MMSE estimator for the absolute value z=cTx+d is defined by formula (8):(10)z^opt=2Pℓπexp−ℓ^22Pℓ+ℓ^1−2Φ−ℓ^Pℓ,where ℓ^ and Pℓ are determined by equation (9).The MMSE estimator (10) allows to calculate distances measured in terms of the absolute value in n-dimensional space.


### 4.2. Examples of MMSE Estimator for Distance between Points

Letxk∈ℝn be a normal state vector, and x^k∈ℝn and Pk∈ℝn×n are the Kalman estimate and error covariance, respectively, Pk=Pij,k.Example 1.
(distance on 1-D line). The MMSE estimator for the distancezk=xk−ak between the moving point xk and given sequence ak in 1-D line takes the form(11)z^kopt=2Pkπexp−x^k−ak22Pk+x^k−ak1−2Φ−x^k−akPk.Example 2.
(distance between point and line). The shortest distancedkℒ,M from the moving point Mxk=Mx1,kx2,k to the line ℒ:Ax1+Bx2+C=0 in 2-D plane is shown in Figure 2. The distance is given by(12)dkℒ,M=Ax1,k+Bx2,k+CA2+B2.
SubstitutingcT=AB and d=C into equations (9) and (10), we get the MMSE estimator for the shortest distance (12):(13)d^koptℒ,M=1A2+B22Pkℓπexp−ℓ^k22Pkℓ+ℓ^k1−2Φ−ℓ^kPkℓ,where ℓ^k and Pkℓ are determined by equation (9):(14)ℓ^k=Ax^1,x+Bx^2,k+C,Pkℓ=A2P11,k+B2P22,k+2ABP12,k.
The MMSE estimator (12)–(14) can be generalized on 3-D space.Figure 2
Shortest distancedkℒ,M from moving point Mxk to 1-D line.Example 3.
(distance between point and plane). Similar to equation (12), the shortest distance between the moving point Mxk=Mx1,k,x2,k,x3,k and the plane P:Ax1+Bx2+Cx3+D=0 in 3-D space,(15)dkP,M=Ax1,k+Bx2,k+Cx3,k+DA2+B2+C2,is shown in Figure 3.
SubstitutingcT=ABC and d=D into equations (9) and (10), we get(16)d^koptP,M=1A2+B2+C22Pkℓπexp−ℓ^k22Pkℓ+ℓ^k1−2Φ−ℓ^kPkℓ,where ℓ^k and Pkℓ are determined by equation (9):(17)ℓ^k=Ax^1,k+Bx^2,k+Cx^3,k+D,Pkℓ=A2P11,k+B2P22,k+C2P33,k+2AP12,k+2ACP13,k+2BCP23,k.
The MMSE distance estimators in Theorem1 and Examples 1∼3 are summarized in Table 2.Figure 3
Shortest distance from pointMx1,x2,x3 to a plane in 3-D.Table 2
MMSE estimators for distance in the Cartesian coordinates.
Definition of distanceMMSE distance estimatorEquationAbsolute valuez=xz^opt=Γx^,P,Γx^,P=2P/πexp−x^2/2P+x^1−2Φ−x^/P.(8)Absolute value of linear formz=cTx+dz^opt=Γ1ℓ^,Pℓ,Γ1ℓ^,Pℓ=2Pℓ/πexp−ℓ^2/2Pℓ+ℓ^1−2Φ−ℓ^/Pℓ,ℓ^=cTx^+d,Pℓ=cTPc(9) and (10)Distance between two points on 1-D linez=x−az^opt=Γ1ℓ^,P,ℓ^=x^−a(11)Distance between pointM and line ℒ in 2-D plane dℒ,M=Ax1+Bx2+C/A2+B2d^ℒ,M=1/A2+B2Γ1ℓ^,Pℓ,ℓ^=Ax^1+Bx^2+C,Pℓ=A2P11+B2P22+2ABP12(12)–(14)Distance between pointM and plane P in 3-D space dP,M=Ax1+Bx2+Cx3+D/A2+B2+C2d^P,M=1/A2+B2+C2Γ1ℓ^,Pℓ,ℓ^=Ax^1+Bx^2+Cx^3+D,Pℓ=A2P11+B2P22+C2P33+2ABP12+2ACP13+2BCP23(15)–(17)


### 4.3. Numerical Examples

In this section, numerical examples demonstrate the accuracy of the two closed-form estimators calculated for the absolute valuez=cTx. The optimal MMSE estimator z^opt is compared with the simple suboptimal one z^sub=cTx^.


#### 4.3.1. Estimation of Distance between Random Location and Given Point in 1-D Line

Letxk be a scalar random position measured in additive white noise; then, the system model is(18)xk+1=xk+vk,x0=m0,yk=xk+wk,k=1,2,…,where m0 is the known initial condition and vk∼ℕ0,q and wk∼ℕ0,r are the uncorrelated white Gaussian noises.The KF equation (3) gives the following:(19)x^k+1=x^k+Kk+1yk+1−x^k,Pk+1−=Pk+q,P0=σ02=1,Kk+1=Pk+1−r+Pk+1−,Pk+1=1−Kk+1Pk+1−,k=0,1,….Consider the distance betweenxk and the known point a, i.e., zk=xk−a. Then, the optimal MMSE estimate of the distance is defined by (10). Further we are interested in the special case in which a=0 and zk=xk. In this case, formula (10) represents the optimal estimate of the distance between the current position xk and the origin point, i.e.,(20)z^kopt=2Pkπexp−αk+x^k1−2Φ−βκ,αk=x^k22Pk,βk=x^kPk.In parallel to the optimal estimate (20), consider the simple suboptimal estimate, z^ksub=x^k.Remark 2.
Reviewing formula (20), we find the following. If the values of αk and βk are large αk,βk≫1, then exp−αk≈0 and Φ−βk≈0 if x^k>0 or Φ−βk≈1 if x^k<0, which implies that both estimates are quite close, i.e., z^kopt≈z^ksub=x^k. Assuming the estimate x^k is far enough from zero, then the large values of the functions αk and βk depend on the error variance Pk. Using (19), the steady-state value of the variance P∞ satisfies the quadratic equation P∞2+qP∞−rq=0 with solution P∞=−q+q2+4rq/2. Since the variance P∞=P∞q,r depends on the noise statistics q and r, this fact can be used in practice to compare the proposed estimators. For example, if the estimate x^k is far enough from zero and the product rq is small rq≪1, then P∞≈0, and αk,βk≫1. In this case, both estimators are close, z^kopt≈z^ksub. Simulation results confirm this result.
Next, we test the efficiency of the proposed estimators. The estimators are compared under different values of the noise variancesq and r. The following scenarios were considered:Case 1: small noises,q=10−4,r=10−2Case 2: medium noises,q=0.1,r=0.5Case 3: large noises,q=0.5,r=1
Both estimators were run with the same random noises for further comparison. The Monte Carlo simulation with 1000 runs was applied in calculation of the root mean square error (RMSE),RMSEkopt=Ezk−z^kopt2, and RMSEksub=Ezk−z^ksub2. Define the average RMSE over the time interval k∈k1,k2 as(21)R¯z^=def1k2−k1+1∑k=k1k2RMSEk.
The simulation results are illustrated in Table3 and Figures 4∼7.
In Case 1, interest is zero and nonzero initial conditionx0=m0.Atm0=0 and q=10−4, the signal xk and its estimate x^k are close to zero, and Pk≈0.001 at k>8. In this case, the values of αk and βk are not large; therefore, the optimal and suboptimal estimates are different as shown in Figure 4 and confirmed by the values R¯z^opt=0.0213 and R¯z^sub=0.0327 in Table 3.Atm0=1 and q=10−4, the estimate x^k is far enough from zero, and Pk≈0.001. According to Remark 1, the optimal and suboptimal estimates are approximately equal, z^kopt≈z^ksub, as shown in Figure 5. The equal values R¯z^opt=R¯z^sub=0.0334 confirm the fact.
In Cases 2 and 3, the varianceP∞ is not small; therefore, the initial condition m0 does not play a significant role in comparing both estimators. In these cases, the optimal estimator z^kopt has better performance than the simple suboptimal one z^ksub=x^k. Typical graphics are shown in Figures 6 and 7, and the values R¯z^opt and R¯z^sub in Table 3 confirm that conclusion.
Thus, the simulation results in Section4.3.1 show that the optimal estimator is suitable for practical applications.Table 3
Simulation results for Section 4.3.1.
Case 1: small noisesq=0.0001r=0.001P∞≈0.001m0=0R¯z^opt=0.0213R¯z^sub=0.0327Figures4(a) and 4(b)m0=1R¯z^opt=0.0334R¯z^sub=0.0334Figures5(a) and 5(b)Case 2: medium noisesq=0.1r=0.5P∞≈0.18m0=0R¯z^opt=0.3110R¯z^sub=0.3883Figures6(a) and 6(b)m0=1R¯z^opt=0.3047R¯z^sub=0.3201Case 3: large noisesq=0.5r=1P∞≈0.5m0=0R¯z^opt=0.6296R¯z^sub=0.6991Figures7(a) and 7(b)m0=1R¯z^opt=0.5359R¯z^sub=0.5798Figure 4
Comparison between optimal and suboptimal estimators for small noises (Case 1) withq=0.0001,r=0.01,P∞≈0.001, and zero initial condition m0=0. (a) True and estimated values for zk=xk. (b) RMSEs for both estimators.
(a)(b)Figure 5
Comparison between optimal and suboptimal estimators for small noises (Case 1) withq=0.0001,r=0.01,P∞=0.001, and nonzero initial condition m0=1. (a) True and very close estimates for zk=xk. (b) Very close RMSE values for both estimators.
(a)(b)Figure 6
Comparison between optimal and suboptimal estimators for medium noises (Case 2) withq=0.1,r=0.5,P∞=0.18, and zero initial condition m0=0. (a) True and estimated values for zk=xk. (b) RMSEs for both estimators.
(a)(b)Figure 7
Comparison between optimal and suboptimal estimators for large noises (Case 3) withq=0.5,r=1.0,P∞=0.5, and zero initial condition m0=0. (a) True and estimated values for zk=xk. (b) RMSEs for both estimators.
(a)(b)


#### 4.3.2. Estimation of Distance between Two Random Points in 1-D Line

Consider a motion of two random pointsA1x1 and A2x2 in 1-D line. Assume that evolution of the state vector xk=x1,kx2,kT from time tk to tk+1 is defined by the random walk model:(22)x1,k+1=x1,k+v1,k,x1,0=m1,k=0,1,…,x2,k+1=x2,k+v2,k,x2,0=m2,where m1 and m2 are the known initial conditions and v1,k∼ℕ0,q1 and v2,k∼ℕ0,q2 are uncorrelated white Gaussian noises.Assuming we measure the true position of the points with correlated measurement white noisesw1 and w2, respectively, the measurement equation is(23)y1,k=x1,k+w1,k,w1,k∼ℕ0,r1,y2,k=x2,k+w2,k,w2,k∼ℕ0,r2,where Ew1,kw2,k=r12.Our goal is to estimate the unknown distancedA1,A2=x1,k−x2,k between the current location of the points A1x1,k and A2x2,k.According to the proposed two-step estimation procedure, the optimal Kalman estimatex^k=x^1,kx^2,kT and error covariance Pk=Pij,k,P0=I2 computed at the first stage are used at the second stage for estimation of the distance zk=x1,k−x2,k. Using formulas (9) and (10) for cT=1−1 and d=0, we obtain the best MMSE estimate for the distance:(24)z^kopt=2Pkℓπexp−αk+ℓ^k1−2Φ−βk,αk=ℓ^k22Pkℓ,βk=ℓ^kPkℓ,ℓ^k=x^1,k−x^2,k,Pkℓ=P11,k+P22,k−2P12,k.In parallel with the optimal distance estimator (24), we consider the simple suboptimal estimator z^ksub=x^1,k−x^2,k.Remark 3.
As we see, the optimal estimatez^kopt of the distances in (20) and (24) depends on the functions αk and βk. The functions in formulas (20) and (24) are calculated in the pairs of points x^k,Pk and ℓ^k,Pkℓ, respectively. The second pair depends on the state estimate x^k=x^1,kx^2,kT and error covariance Pk=Pij,k. Therefore, Remark 2 is also valid for models (22) and (23). For example, if the estimate ℓ^k=x^1,k−x^2,k is far enough from zero and the variance Pkℓ=Eℓk−ℓ^k2 is small, then z^kopt≈z^ksub. The simulation results in Figure 8 with Pkℓ=0.0015,k>8, and very close values of the average RMSEs, R¯z^opt=0.0189 and R¯z^sub=0.0189, confirm this fact.
In addition, we are interested in the following new scenarios:Case 1: both pointsA1x1 and A2x2 are fixed, and their positions are measured with small noisesCase 2: the first pointA1x1 is fixed, but the movement of the second one A2x2 is subject to a small noiseCase 3: the movement of both points is subject to a medium noise
The model parameters and simulation results for the scenarios are illustrated in Table4. From Table 4, we observe the strong difference between the average RMSEs R¯z^opt and R¯z^sub, i.e., R¯z^opt<R¯z^sub. It is not a surprise that the optimal estimator (24) is better than the suboptimal one, z^ksub=x^1,k−x^2,k.Figure 8
Comparison between optimal and suboptimal estimators for small noises withq1=q2=10−4,r1=r2=10−2,r12=0.005, and initial positions m1=0,m12=1. (a) True and very close estimates for the distance zk=x1,k−x2,k. (b) Very close RMSE values for both estimators.
(a)(b)Table 4
Simulation results for Section 4.3.2.
Case 1m1=1;m2=1.01q1=0;q2=0r1=0.01;r2=0.01;r12=0.005P∞ℓ=0.0001R¯z^opt=0.0013R¯z^sub=0.0060Case 2m1=1;m2=1.01q1=0;q2=0.01r1=0.01;r2=0.01;r12=0.005P∞ℓ=0.015R¯z^opt=0.0378R¯z^sub=0.0450Case 3m1=1;m2=1.01q1=0.05;q2=0.03r1=0.05;r2=0.05;r12=0.001P∞ℓ=0.1366R¯z^opt=0.1596R¯z^sub=0.1796


## 4.1. MMSE Estimator for Absolute Value of Linear Form

Lemma 1.
(MMSE estimator for |x|). Let x∈ℝ be a normal random variable, and x^ and P=Ex−x^2 are the MMSE estimate and error variance, respectively. Then, the MMSE estimator for the absolute value z=x has the following closed-form expression:(8)z^opt=Exyk=2Pπexp−x^22P+x^1−2Φ−x^P,where Φ⋅ is the cumulative distribution function of the standard normal distribution, ℕ0,1.The derivation of equation (8) is given in the Appendix.Letℓ=cTx+d be a linear form (LF) of the normal random vector, x∈ℝn, and x^∈ℝn and P∈ℝn×n are the MMSE estimate and error covariance, respectively. Then, the MMSE estimate of the linear form ℓ and its error variance can be calculated as(9)ℓ^=Eℓyk=cTx^+d,Pℓ=Eℓ−ℓ^2=cTPc,and we have the following theorem.Theorem 1.
(MMSE estimator for absolute value of LF). Letx∈ℝn be a normal random vector, and x^∈ℝn and P∈ℝn×n are the MMSE estimate and error covariance, respectively. Then, the closed-form MMSE estimator for the absolute value z=cTx+d is defined by formula (8):(10)z^opt=2Pℓπexp−ℓ^22Pℓ+ℓ^1−2Φ−ℓ^Pℓ,where ℓ^ and Pℓ are determined by equation (9).The MMSE estimator (10) allows to calculate distances measured in terms of the absolute value in n-dimensional space.


## 4.2. Examples of MMSE Estimator for Distance between Points

Letxk∈ℝn be a normal state vector, and x^k∈ℝn and Pk∈ℝn×n are the Kalman estimate and error covariance, respectively, Pk=Pij,k.Example 1.
(distance on 1-D line). The MMSE estimator for the distancezk=xk−ak between the moving point xk and given sequence ak in 1-D line takes the form(11)z^kopt=2Pkπexp−x^k−ak22Pk+x^k−ak1−2Φ−x^k−akPk.Example 2.
(distance between point and line). The shortest distancedkℒ,M from the moving point Mxk=Mx1,kx2,k to the line ℒ:Ax1+Bx2+C=0 in 2-D plane is shown in Figure 2. The distance is given by(12)dkℒ,M=Ax1,k+Bx2,k+CA2+B2.
SubstitutingcT=AB and d=C into equations (9) and (10), we get the MMSE estimator for the shortest distance (12):(13)d^koptℒ,M=1A2+B22Pkℓπexp−ℓ^k22Pkℓ+ℓ^k1−2Φ−ℓ^kPkℓ,where ℓ^k and Pkℓ are determined by equation (9):(14)ℓ^k=Ax^1,x+Bx^2,k+C,Pkℓ=A2P11,k+B2P22,k+2ABP12,k.
The MMSE estimator (12)–(14) can be generalized on 3-D space.Figure 2
Shortest distancedkℒ,M from moving point Mxk to 1-D line.Example 3.
(distance between point and plane). Similar to equation (12), the shortest distance between the moving point Mxk=Mx1,k,x2,k,x3,k and the plane P:Ax1+Bx2+Cx3+D=0 in 3-D space,(15)dkP,M=Ax1,k+Bx2,k+Cx3,k+DA2+B2+C2,is shown in Figure 3.
SubstitutingcT=ABC and d=D into equations (9) and (10), we get(16)d^koptP,M=1A2+B2+C22Pkℓπexp−ℓ^k22Pkℓ+ℓ^k1−2Φ−ℓ^kPkℓ,where ℓ^k and Pkℓ are determined by equation (9):(17)ℓ^k=Ax^1,k+Bx^2,k+Cx^3,k+D,Pkℓ=A2P11,k+B2P22,k+C2P33,k+2AP12,k+2ACP13,k+2BCP23,k.
The MMSE distance estimators in Theorem1 and Examples 1∼3 are summarized in Table 2.Figure 3
Shortest distance from pointMx1,x2,x3 to a plane in 3-D.Table 2
MMSE estimators for distance in the Cartesian coordinates.
Definition of distanceMMSE distance estimatorEquationAbsolute valuez=xz^opt=Γx^,P,Γx^,P=2P/πexp−x^2/2P+x^1−2Φ−x^/P.(8)Absolute value of linear formz=cTx+dz^opt=Γ1ℓ^,Pℓ,Γ1ℓ^,Pℓ=2Pℓ/πexp−ℓ^2/2Pℓ+ℓ^1−2Φ−ℓ^/Pℓ,ℓ^=cTx^+d,Pℓ=cTPc(9) and (10)Distance between two points on 1-D linez=x−az^opt=Γ1ℓ^,P,ℓ^=x^−a(11)Distance between pointM and line ℒ in 2-D plane dℒ,M=Ax1+Bx2+C/A2+B2d^ℒ,M=1/A2+B2Γ1ℓ^,Pℓ,ℓ^=Ax^1+Bx^2+C,Pℓ=A2P11+B2P22+2ABP12(12)–(14)Distance between pointM and plane P in 3-D space dP,M=Ax1+Bx2+Cx3+D/A2+B2+C2d^P,M=1/A2+B2+C2Γ1ℓ^,Pℓ,ℓ^=Ax^1+Bx^2+Cx^3+D,Pℓ=A2P11+B2P22+C2P33+2ABP12+2ACP13+2BCP23(15)–(17)


## 4.3. Numerical Examples

In this section, numerical examples demonstrate the accuracy of the two closed-form estimators calculated for the absolute valuez=cTx. The optimal MMSE estimator z^opt is compared with the simple suboptimal one z^sub=cTx^.


### 4.3.1. Estimation of Distance between Random Location and Given Point in 1-D Line

Letxk be a scalar random position measured in additive white noise; then, the system model is(18)xk+1=xk+vk,x0=m0,yk=xk+wk,k=1,2,…,where m0 is the known initial condition and vk∼ℕ0,q and wk∼ℕ0,r are the uncorrelated white Gaussian noises.The KF equation (3) gives the following:(19)x^k+1=x^k+Kk+1yk+1−x^k,Pk+1−=Pk+q,P0=σ02=1,Kk+1=Pk+1−r+Pk+1−,Pk+1=1−Kk+1Pk+1−,k=0,1,….Consider the distance betweenxk and the known point a, i.e., zk=xk−a. Then, the optimal MMSE estimate of the distance is defined by (10). Further we are interested in the special case in which a=0 and zk=xk. In this case, formula (10) represents the optimal estimate of the distance between the current position xk and the origin point, i.e.,(20)z^kopt=2Pkπexp−αk+x^k1−2Φ−βκ,αk=x^k22Pk,βk=x^kPk.In parallel to the optimal estimate (20), consider the simple suboptimal estimate, z^ksub=x^k.Remark 2.
Reviewing formula (20), we find the following. If the values of αk and βk are large αk,βk≫1, then exp−αk≈0 and Φ−βk≈0 if x^k>0 or Φ−βk≈1 if x^k<0, which implies that both estimates are quite close, i.e., z^kopt≈z^ksub=x^k. Assuming the estimate x^k is far enough from zero, then the large values of the functions αk and βk depend on the error variance Pk. Using (19), the steady-state value of the variance P∞ satisfies the quadratic equation P∞2+qP∞−rq=0 with solution P∞=−q+q2+4rq/2. Since the variance P∞=P∞q,r depends on the noise statistics q and r, this fact can be used in practice to compare the proposed estimators. For example, if the estimate x^k is far enough from zero and the product rq is small rq≪1, then P∞≈0, and αk,βk≫1. In this case, both estimators are close, z^kopt≈z^ksub. Simulation results confirm this result.
Next, we test the efficiency of the proposed estimators. The estimators are compared under different values of the noise variancesq and r. The following scenarios were considered:Case 1: small noises,q=10−4,r=10−2Case 2: medium noises,q=0.1,r=0.5Case 3: large noises,q=0.5,r=1
Both estimators were run with the same random noises for further comparison. The Monte Carlo simulation with 1000 runs was applied in calculation of the root mean square error (RMSE),RMSEkopt=Ezk−z^kopt2, and RMSEksub=Ezk−z^ksub2. Define the average RMSE over the time interval k∈k1,k2 as(21)R¯z^=def1k2−k1+1∑k=k1k2RMSEk.
The simulation results are illustrated in Table3 and Figures 4∼7.
In Case 1, interest is zero and nonzero initial conditionx0=m0.Atm0=0 and q=10−4, the signal xk and its estimate x^k are close to zero, and Pk≈0.001 at k>8. In this case, the values of αk and βk are not large; therefore, the optimal and suboptimal estimates are different as shown in Figure 4 and confirmed by the values R¯z^opt=0.0213 and R¯z^sub=0.0327 in Table 3.Atm0=1 and q=10−4, the estimate x^k is far enough from zero, and Pk≈0.001. According to Remark 1, the optimal and suboptimal estimates are approximately equal, z^kopt≈z^ksub, as shown in Figure 5. The equal values R¯z^opt=R¯z^sub=0.0334 confirm the fact.
In Cases 2 and 3, the varianceP∞ is not small; therefore, the initial condition m0 does not play a significant role in comparing both estimators. In these cases, the optimal estimator z^kopt has better performance than the simple suboptimal one z^ksub=x^k. Typical graphics are shown in Figures 6 and 7, and the values R¯z^opt and R¯z^sub in Table 3 confirm that conclusion.
Thus, the simulation results in Section4.3.1 show that the optimal estimator is suitable for practical applications.Table 3
Simulation results for Section 4.3.1.
Case 1: small noisesq=0.0001r=0.001P∞≈0.001m0=0R¯z^opt=0.0213R¯z^sub=0.0327Figures4(a) and 4(b)m0=1R¯z^opt=0.0334R¯z^sub=0.0334Figures5(a) and 5(b)Case 2: medium noisesq=0.1r=0.5P∞≈0.18m0=0R¯z^opt=0.3110R¯z^sub=0.3883Figures6(a) and 6(b)m0=1R¯z^opt=0.3047R¯z^sub=0.3201Case 3: large noisesq=0.5r=1P∞≈0.5m0=0R¯z^opt=0.6296R¯z^sub=0.6991Figures7(a) and 7(b)m0=1R¯z^opt=0.5359R¯z^sub=0.5798Figure 4
Comparison between optimal and suboptimal estimators for small noises (Case 1) withq=0.0001,r=0.01,P∞≈0.001, and zero initial condition m0=0. (a) True and estimated values for zk=xk. (b) RMSEs for both estimators.
(a)(b)Figure 5
Comparison between optimal and suboptimal estimators for small noises (Case 1) withq=0.0001,r=0.01,P∞=0.001, and nonzero initial condition m0=1. (a) True and very close estimates for zk=xk. (b) Very close RMSE values for both estimators.
(a)(b)Figure 6
Comparison between optimal and suboptimal estimators for medium noises (Case 2) withq=0.1,r=0.5,P∞=0.18, and zero initial condition m0=0. (a) True and estimated values for zk=xk. (b) RMSEs for both estimators.
(a)(b)Figure 7
Comparison between optimal and suboptimal estimators for large noises (Case 3) withq=0.5,r=1.0,P∞=0.5, and zero initial condition m0=0. (a) True and estimated values for zk=xk. (b) RMSEs for both estimators.
(a)(b)


### 4.3.2. Estimation of Distance between Two Random Points in 1-D Line

Consider a motion of two random pointsA1x1 and A2x2 in 1-D line. Assume that evolution of the state vector xk=x1,kx2,kT from time tk to tk+1 is defined by the random walk model:(22)x1,k+1=x1,k+v1,k,x1,0=m1,k=0,1,…,x2,k+1=x2,k+v2,k,x2,0=m2,where m1 and m2 are the known initial conditions and v1,k∼ℕ0,q1 and v2,k∼ℕ0,q2 are uncorrelated white Gaussian noises.Assuming we measure the true position of the points with correlated measurement white noisesw1 and w2, respectively, the measurement equation is(23)y1,k=x1,k+w1,k,w1,k∼ℕ0,r1,y2,k=x2,k+w2,k,w2,k∼ℕ0,r2,where Ew1,kw2,k=r12.Our goal is to estimate the unknown distancedA1,A2=x1,k−x2,k between the current location of the points A1x1,k and A2x2,k.According to the proposed two-step estimation procedure, the optimal Kalman estimatex^k=x^1,kx^2,kT and error covariance Pk=Pij,k,P0=I2 computed at the first stage are used at the second stage for estimation of the distance zk=x1,k−x2,k. Using formulas (9) and (10) for cT=1−1 and d=0, we obtain the best MMSE estimate for the distance:(24)z^kopt=2Pkℓπexp−αk+ℓ^k1−2Φ−βk,αk=ℓ^k22Pkℓ,βk=ℓ^kPkℓ,ℓ^k=x^1,k−x^2,k,Pkℓ=P11,k+P22,k−2P12,k.In parallel with the optimal distance estimator (24), we consider the simple suboptimal estimator z^ksub=x^1,k−x^2,k.Remark 3.
As we see, the optimal estimatez^kopt of the distances in (20) and (24) depends on the functions αk and βk. The functions in formulas (20) and (24) are calculated in the pairs of points x^k,Pk and ℓ^k,Pkℓ, respectively. The second pair depends on the state estimate x^k=x^1,kx^2,kT and error covariance Pk=Pij,k. Therefore, Remark 2 is also valid for models (22) and (23). For example, if the estimate ℓ^k=x^1,k−x^2,k is far enough from zero and the variance Pkℓ=Eℓk−ℓ^k2 is small, then z^kopt≈z^ksub. The simulation results in Figure 8 with Pkℓ=0.0015,k>8, and very close values of the average RMSEs, R¯z^opt=0.0189 and R¯z^sub=0.0189, confirm this fact.
In addition, we are interested in the following new scenarios:Case 1: both pointsA1x1 and A2x2 are fixed, and their positions are measured with small noisesCase 2: the first pointA1x1 is fixed, but the movement of the second one A2x2 is subject to a small noiseCase 3: the movement of both points is subject to a medium noise
The model parameters and simulation results for the scenarios are illustrated in Table4. From Table 4, we observe the strong difference between the average RMSEs R¯z^opt and R¯z^sub, i.e., R¯z^opt<R¯z^sub. It is not a surprise that the optimal estimator (24) is better than the suboptimal one, z^ksub=x^1,k−x^2,k.Figure 8
Comparison between optimal and suboptimal estimators for small noises withq1=q2=10−4,r1=r2=10−2,r12=0.005, and initial positions m1=0,m12=1. (a) True and very close estimates for the distance zk=x1,k−x2,k. (b) Very close RMSE values for both estimators.
(a)(b)Table 4
Simulation results for Section 4.3.2.
Case 1m1=1;m2=1.01q1=0;q2=0r1=0.01;r2=0.01;r12=0.005P∞ℓ=0.0001R¯z^opt=0.0013R¯z^sub=0.0060Case 2m1=1;m2=1.01q1=0;q2=0.01r1=0.01;r2=0.01;r12=0.005P∞ℓ=0.015R¯z^opt=0.0378R¯z^sub=0.0450Case 3m1=1;m2=1.01q1=0.05;q2=0.03r1=0.05;r2=0.05;r12=0.001P∞ℓ=0.1366R¯z^opt=0.1596R¯z^sub=0.1796


## 4.3.1. Estimation of Distance between Random Location and Given Point in 1-D Line

Letxk be a scalar random position measured in additive white noise; then, the system model is(18)xk+1=xk+vk,x0=m0,yk=xk+wk,k=1,2,…,where m0 is the known initial condition and vk∼ℕ0,q and wk∼ℕ0,r are the uncorrelated white Gaussian noises.The KF equation (3) gives the following:(19)x^k+1=x^k+Kk+1yk+1−x^k,Pk+1−=Pk+q,P0=σ02=1,Kk+1=Pk+1−r+Pk+1−,Pk+1=1−Kk+1Pk+1−,k=0,1,….Consider the distance betweenxk and the known point a, i.e., zk=xk−a. Then, the optimal MMSE estimate of the distance is defined by (10). Further we are interested in the special case in which a=0 and zk=xk. In this case, formula (10) represents the optimal estimate of the distance between the current position xk and the origin point, i.e.,(20)z^kopt=2Pkπexp−αk+x^k1−2Φ−βκ,αk=x^k22Pk,βk=x^kPk.In parallel to the optimal estimate (20), consider the simple suboptimal estimate, z^ksub=x^k.Remark 2.
Reviewing formula (20), we find the following. If the values of αk and βk are large αk,βk≫1, then exp−αk≈0 and Φ−βk≈0 if x^k>0 or Φ−βk≈1 if x^k<0, which implies that both estimates are quite close, i.e., z^kopt≈z^ksub=x^k. Assuming the estimate x^k is far enough from zero, then the large values of the functions αk and βk depend on the error variance Pk. Using (19), the steady-state value of the variance P∞ satisfies the quadratic equation P∞2+qP∞−rq=0 with solution P∞=−q+q2+4rq/2. Since the variance P∞=P∞q,r depends on the noise statistics q and r, this fact can be used in practice to compare the proposed estimators. For example, if the estimate x^k is far enough from zero and the product rq is small rq≪1, then P∞≈0, and αk,βk≫1. In this case, both estimators are close, z^kopt≈z^ksub. Simulation results confirm this result.
Next, we test the efficiency of the proposed estimators. The estimators are compared under different values of the noise variancesq and r. The following scenarios were considered:Case 1: small noises,q=10−4,r=10−2Case 2: medium noises,q=0.1,r=0.5Case 3: large noises,q=0.5,r=1
Both estimators were run with the same random noises for further comparison. The Monte Carlo simulation with 1000 runs was applied in calculation of the root mean square error (RMSE),RMSEkopt=Ezk−z^kopt2, and RMSEksub=Ezk−z^ksub2. Define the average RMSE over the time interval k∈k1,k2 as(21)R¯z^=def1k2−k1+1∑k=k1k2RMSEk.
The simulation results are illustrated in Table3 and Figures 4∼7.
In Case 1, interest is zero and nonzero initial conditionx0=m0.Atm0=0 and q=10−4, the signal xk and its estimate x^k are close to zero, and Pk≈0.001 at k>8. In this case, the values of αk and βk are not large; therefore, the optimal and suboptimal estimates are different as shown in Figure 4 and confirmed by the values R¯z^opt=0.0213 and R¯z^sub=0.0327 in Table 3.Atm0=1 and q=10−4, the estimate x^k is far enough from zero, and Pk≈0.001. According to Remark 1, the optimal and suboptimal estimates are approximately equal, z^kopt≈z^ksub, as shown in Figure 5. The equal values R¯z^opt=R¯z^sub=0.0334 confirm the fact.
In Cases 2 and 3, the varianceP∞ is not small; therefore, the initial condition m0 does not play a significant role in comparing both estimators. In these cases, the optimal estimator z^kopt has better performance than the simple suboptimal one z^ksub=x^k. Typical graphics are shown in Figures 6 and 7, and the values R¯z^opt and R¯z^sub in Table 3 confirm that conclusion.
Thus, the simulation results in Section4.3.1 show that the optimal estimator is suitable for practical applications.Table 3
Simulation results for Section 4.3.1.
Case 1: small noisesq=0.0001r=0.001P∞≈0.001m0=0R¯z^opt=0.0213R¯z^sub=0.0327Figures4(a) and 4(b)m0=1R¯z^opt=0.0334R¯z^sub=0.0334Figures5(a) and 5(b)Case 2: medium noisesq=0.1r=0.5P∞≈0.18m0=0R¯z^opt=0.3110R¯z^sub=0.3883Figures6(a) and 6(b)m0=1R¯z^opt=0.3047R¯z^sub=0.3201Case 3: large noisesq=0.5r=1P∞≈0.5m0=0R¯z^opt=0.6296R¯z^sub=0.6991Figures7(a) and 7(b)m0=1R¯z^opt=0.5359R¯z^sub=0.5798Figure 4
Comparison between optimal and suboptimal estimators for small noises (Case 1) withq=0.0001,r=0.01,P∞≈0.001, and zero initial condition m0=0. (a) True and estimated values for zk=xk. (b) RMSEs for both estimators.
(a)(b)Figure 5
Comparison between optimal and suboptimal estimators for small noises (Case 1) withq=0.0001,r=0.01,P∞=0.001, and nonzero initial condition m0=1. (a) True and very close estimates for zk=xk. (b) Very close RMSE values for both estimators.
(a)(b)Figure 6
Comparison between optimal and suboptimal estimators for medium noises (Case 2) withq=0.1,r=0.5,P∞=0.18, and zero initial condition m0=0. (a) True and estimated values for zk=xk. (b) RMSEs for both estimators.
(a)(b)Figure 7
Comparison between optimal and suboptimal estimators for large noises (Case 3) withq=0.5,r=1.0,P∞=0.5, and zero initial condition m0=0. (a) True and estimated values for zk=xk. (b) RMSEs for both estimators.
(a)(b)


## 4.3.2. Estimation of Distance between Two Random Points in 1-D Line

Consider a motion of two random pointsA1x1 and A2x2 in 1-D line. Assume that evolution of the state vector xk=x1,kx2,kT from time tk to tk+1 is defined by the random walk model:(22)x1,k+1=x1,k+v1,k,x1,0=m1,k=0,1,…,x2,k+1=x2,k+v2,k,x2,0=m2,where m1 and m2 are the known initial conditions and v1,k∼ℕ0,q1 and v2,k∼ℕ0,q2 are uncorrelated white Gaussian noises.Assuming we measure the true position of the points with correlated measurement white noisesw1 and w2, respectively, the measurement equation is(23)y1,k=x1,k+w1,k,w1,k∼ℕ0,r1,y2,k=x2,k+w2,k,w2,k∼ℕ0,r2,where Ew1,kw2,k=r12.Our goal is to estimate the unknown distancedA1,A2=x1,k−x2,k between the current location of the points A1x1,k and A2x2,k.According to the proposed two-step estimation procedure, the optimal Kalman estimatex^k=x^1,kx^2,kT and error covariance Pk=Pij,k,P0=I2 computed at the first stage are used at the second stage for estimation of the distance zk=x1,k−x2,k. Using formulas (9) and (10) for cT=1−1 and d=0, we obtain the best MMSE estimate for the distance:(24)z^kopt=2Pkℓπexp−αk+ℓ^k1−2Φ−βk,αk=ℓ^k22Pkℓ,βk=ℓ^kPkℓ,ℓ^k=x^1,k−x^2,k,Pkℓ=P11,k+P22,k−2P12,k.In parallel with the optimal distance estimator (24), we consider the simple suboptimal estimator z^ksub=x^1,k−x^2,k.Remark 3.
As we see, the optimal estimatez^kopt of the distances in (20) and (24) depends on the functions αk and βk. The functions in formulas (20) and (24) are calculated in the pairs of points x^k,Pk and ℓ^k,Pkℓ, respectively. The second pair depends on the state estimate x^k=x^1,kx^2,kT and error covariance Pk=Pij,k. Therefore, Remark 2 is also valid for models (22) and (23). For example, if the estimate ℓ^k=x^1,k−x^2,k is far enough from zero and the variance Pkℓ=Eℓk−ℓ^k2 is small, then z^kopt≈z^ksub. The simulation results in Figure 8 with Pkℓ=0.0015,k>8, and very close values of the average RMSEs, R¯z^opt=0.0189 and R¯z^sub=0.0189, confirm this fact.
In addition, we are interested in the following new scenarios:Case 1: both pointsA1x1 and A2x2 are fixed, and their positions are measured with small noisesCase 2: the first pointA1x1 is fixed, but the movement of the second one A2x2 is subject to a small noiseCase 3: the movement of both points is subject to a medium noise
The model parameters and simulation results for the scenarios are illustrated in Table4. From Table 4, we observe the strong difference between the average RMSEs R¯z^opt and R¯z^sub, i.e., R¯z^opt<R¯z^sub. It is not a surprise that the optimal estimator (24) is better than the suboptimal one, z^ksub=x^1,k−x^2,k.Figure 8
Comparison between optimal and suboptimal estimators for small noises withq1=q2=10−4,r1=r2=10−2,r12=0.005, and initial positions m1=0,m12=1. (a) True and very close estimates for the distance zk=x1,k−x2,k. (b) Very close RMSE values for both estimators.
(a)(b)Table 4
Simulation results for Section 4.3.2.
Case 1m1=1;m2=1.01q1=0;q2=0r1=0.01;r2=0.01;r12=0.005P∞ℓ=0.0001R¯z^opt=0.0013R¯z^sub=0.0060Case 2m1=1;m2=1.01q1=0;q2=0.01r1=0.01;r2=0.01;r12=0.005P∞ℓ=0.015R¯z^opt=0.0378R¯z^sub=0.0450Case 3m1=1;m2=1.01q1=0.05;q2=0.03r1=0.05;r2=0.05;r12=0.001P∞ℓ=0.1366R¯z^opt=0.1596R¯z^sub=0.1796


## 5. MMSE Estimators for Bilinear and Quadratic Forms

### 5.1. Optimal Closed-Form MMSE Estimator for Quadratic Form

Consider a quadratic form (QF) of the state vectorxk∈ℝn:(25)zk=xkTAkxk,Ak=AkT.In this case, the optimal MMSE estimator (4) can be explicitly calculated in terms of the Kalman estimate x^k and error covariance Pk.Theorem 2.
(MMSE estimator for QF). Letxk∈ℝn be a normal random vector, and x^k∈ℝn and Pk∈ℝn×n are the Kalman estimate and error covariance, respectively. Then, the optimal MMSE estimator for the QF zk=xkTAkxk has the following closed-form structure:(26)z^kopt=x^kTAkx^k+trAkPk.Proof.
Using the formulasxTAx=trAxxT and ExxT=Covx+ExExT, we obtain(27)z^kopt=ExkTAkxkyk=EtrAkxkxkTyk=trAkExkxkTyk=trAkPk+ExkykExkTyk=trAkPk+trAkx^kx^kT=trAkPk+x^kTAkx^k.
In parallel to the optimal quadratic estimator (26), we consider the simple suboptimal estimator denoted as z^ksub, which is obtained by direct calculation of the QF at the point xk=x^k such as(28)z^ksub=x^kTAkx^k.
The simple estimator (28) depends only on the Kalman estimate x^k and does not require the KF error covariance Pk in contrast to the optimal one (26). The following result compares the estimation accuracy of the optimal and suboptimal quadratic estimators.Lemma 2.
(difference between MSEs for quadratic estimators). The difference between the true MSEsPz,kopt=Ezk−z^kopt2 and Pz,ksub=Ezk−z^ksub2 for the optimal and simple suboptimal quadratic estimators is tr2AkPk.Proof.
Using the fact that the MMSE estimator is unbiased,Ez^kopt−zk=0, and the equality z^ksub=z^kopt−trAkPk, we obtain(29)Pz,ksub=Ezk−z^ksub2=Ezk−z^kopt+trAkPk2=Ezk−z^kopt2+2trAkPkEzk−z^kopt+tr2AkPk=Pz,kopt+tr2AkPk.
Let us illustrate Theorem2 and Lemma 2 on the example of the squared norm of a random vector, zk=xk2=xkTxk. Then, Ak=In, and the quadratic estimators and difference between their MSEs take the form(30)z^kopt=x^kTx^k+trPk,z^ksub=x^kTx^k,δk=Pz,ksub−Pz,kopt=tr2Pk.
We see the differenceδk=tr2Pk depends on the quality of the KF data processing (3).


### 5.2. Optimal Closed-Form MMSE Estimator for Bilinear Form

Letxk∈ℝn and x˜k∈ℝn be two arbitrary state vectors. Then, a bilinear form (BLF) on the state space can be written as follows:(31)uk=xkTAkx˜k,Ak=AkT.Note that a BLF can be written as a QF in the vectorXk∈ℝ2n. In this case,(32)uk=xkTAkx˜k=xkTx˜kTBkxkx˜k=XkTBkXk,Xk=xkx˜k,XkT=xkTx˜kT,Bk=On12Ak12AkTOn.For the QF (25), the optimal bilinear estimator can be explicitly calculated in terms of the Kalman estimate x^k∈ℝ2n and block error covariance matrix Pk∈ℝ2n×2n:(33)x^k=x^kx˜^κ,Pk=Pxx,kPxx˜,kPxx˜,kTPx˜x˜,k,where Pxx˜,k=Covex,k,ex˜,k is a cross covariance between estimation errors ex,k=xk−x^k and ex˜,k=x˜k−x˜^k.Applying Theorem2 to the QF zk=XkTBkXk and taking into consideration the block structure of the matrix Bk, we have the following.Theorem 3.
(MMSE estimator for BLF). Letxk=xkTx˜kTT∈ℝ2n be a joint normal random vector, and x^k∈ℝ2n and Pk∈ℝ2n×2n are the Kalman estimate and block error covariance matrix (33). Then, the optimal MMSE estimator for the BLF uk=xkTAkx˜k has the following closed-form structure:(34)u^kopt=x^kTAkx˜^k+trAkPxx˜,k.Example 4.
(estimation of inner product and squared Euclidean distance). Using the bilinear estimator (34) with Ak=In, the MMSE estimator for the inner product uk=xkTx˜k takes the form(35)u^kopt=x^kTx˜^k+trPxx˜,k.
Next, calculate the optimal MMSE estimator for the squared Euclidean distance between two pointszk=d2xk,x˜k=xk−x˜k22 or zk=ηk22, where ηk=xk−x˜k. The Kalman estimate and error covariance of the difference ηk take the form(36)η^k=x^k−x˜^k,Pkη=Pxx,k+Px˜x˜,k−Pxx˜,k−Pxx˜,kT.
Applying the quadratic estimator (26) with Ak=In, we obtain the MMSE estimator for the squared Euclidean distance:(37)z^kopt=η^kTη^k+trPkη=x^k−x˜^kTx^k−x˜^k+trPkη.
The MMSE estimators for bilinear and quadratic forms are summarized in Table5.Table 5
MMSE estimators for QF, BLF, inner product, and squared norm.
TitleFormula for functionMMSE estimatorEquationQFz=xTAx,A=ATz^opt=x^TAx^+trAP(26)BLFu=xTBx˜,B=BTu^opt=x^TBx˜^+trBPxx˜(34)Inner productu=x,x˜=xTx˜u^opt=x^Tx˜^+trPxx˜(35)Euclidean distancez=x−x˜22z^opt=x^−x˜^Tx^−x˜^+trPη,Pη=Pxx+Px˜x˜−Pxx˜−Pxx˜T(37)


### 5.3. Practical Usefulness of Squared Euclidean Distance

In many practical problems, for example, finding the shortest distance from a point to a curve,minx,x˜∈ℳdx,x˜, or comparing a distance with a threshold value, dx,x˜≷ε, there is no need to calculate the original Euclidean distance dx,x˜=∑i=1nxi−x˜i2, but we just need to calculate its square due to the equivalence of the problems, minx,x˜∈ℳdx,x˜⇔minx,x˜∈ℳd2x,x˜ or dx,x˜≷ε⟺d2x,x˜≷ε2. In such situations, the optimal quadratic estimator (37) for the squared Euclidean distance, d2x,x˜=∑i=1nxi−x˜i2, can be successfully used.Example 5.
(deviation of normal and nominal trajectories). Suppose that the piecewise feedback control lawUk∗ depends on the difference between a normal xk and nominal xkn trajectories. For example, it is given by(38)Uk∗=1,dxk,xkn<D,−1,otherwise,where dxk,xkn is the Euclidean distance and D is the distance threshold (see Figure 9).
In view of the above, rewrite the control law in the equivalent form:(39)Uk∗=1,dxk,xkn<D2,−1,otherwise,where d2xk,xkn is the squared of the Euclidean distance and D2 is the new threshold.
Using the quadratic estimator (37) for the squared distance zk=d2xk,xkn, we obtain the MMSE estimator, z^kopt=x^k−x^knTx^k−x^kn+trPk, which can be used in the control (39):(40)Uk∗=1,z^kopt<D2,−1,otherwise.
In the next section, we discuss application of the linear, bilinear, and quadratic estimators (Theorems1–3) for estimation of composite nonlinear functions.Figure 9
Distance deviation between normal and nominal (desired) trajectories.


## 5.1. Optimal Closed-Form MMSE Estimator for Quadratic Form

Consider a quadratic form (QF) of the state vectorxk∈ℝn:(25)zk=xkTAkxk,Ak=AkT.In this case, the optimal MMSE estimator (4) can be explicitly calculated in terms of the Kalman estimate x^k and error covariance Pk.Theorem 2.
(MMSE estimator for QF). Letxk∈ℝn be a normal random vector, and x^k∈ℝn and Pk∈ℝn×n are the Kalman estimate and error covariance, respectively. Then, the optimal MMSE estimator for the QF zk=xkTAkxk has the following closed-form structure:(26)z^kopt=x^kTAkx^k+trAkPk.Proof.
Using the formulasxTAx=trAxxT and ExxT=Covx+ExExT, we obtain(27)z^kopt=ExkTAkxkyk=EtrAkxkxkTyk=trAkExkxkTyk=trAkPk+ExkykExkTyk=trAkPk+trAkx^kx^kT=trAkPk+x^kTAkx^k.
In parallel to the optimal quadratic estimator (26), we consider the simple suboptimal estimator denoted as z^ksub, which is obtained by direct calculation of the QF at the point xk=x^k such as(28)z^ksub=x^kTAkx^k.
The simple estimator (28) depends only on the Kalman estimate x^k and does not require the KF error covariance Pk in contrast to the optimal one (26). The following result compares the estimation accuracy of the optimal and suboptimal quadratic estimators.Lemma 2.
(difference between MSEs for quadratic estimators). The difference between the true MSEsPz,kopt=Ezk−z^kopt2 and Pz,ksub=Ezk−z^ksub2 for the optimal and simple suboptimal quadratic estimators is tr2AkPk.Proof.
Using the fact that the MMSE estimator is unbiased,Ez^kopt−zk=0, and the equality z^ksub=z^kopt−trAkPk, we obtain(29)Pz,ksub=Ezk−z^ksub2=Ezk−z^kopt+trAkPk2=Ezk−z^kopt2+2trAkPkEzk−z^kopt+tr2AkPk=Pz,kopt+tr2AkPk.
Let us illustrate Theorem2 and Lemma 2 on the example of the squared norm of a random vector, zk=xk2=xkTxk. Then, Ak=In, and the quadratic estimators and difference between their MSEs take the form(30)z^kopt=x^kTx^k+trPk,z^ksub=x^kTx^k,δk=Pz,ksub−Pz,kopt=tr2Pk.
We see the differenceδk=tr2Pk depends on the quality of the KF data processing (3).


## 5.2. Optimal Closed-Form MMSE Estimator for Bilinear Form

Letxk∈ℝn and x˜k∈ℝn be two arbitrary state vectors. Then, a bilinear form (BLF) on the state space can be written as follows:(31)uk=xkTAkx˜k,Ak=AkT.Note that a BLF can be written as a QF in the vectorXk∈ℝ2n. In this case,(32)uk=xkTAkx˜k=xkTx˜kTBkxkx˜k=XkTBkXk,Xk=xkx˜k,XkT=xkTx˜kT,Bk=On12Ak12AkTOn.For the QF (25), the optimal bilinear estimator can be explicitly calculated in terms of the Kalman estimate x^k∈ℝ2n and block error covariance matrix Pk∈ℝ2n×2n:(33)x^k=x^kx˜^κ,Pk=Pxx,kPxx˜,kPxx˜,kTPx˜x˜,k,where Pxx˜,k=Covex,k,ex˜,k is a cross covariance between estimation errors ex,k=xk−x^k and ex˜,k=x˜k−x˜^k.Applying Theorem2 to the QF zk=XkTBkXk and taking into consideration the block structure of the matrix Bk, we have the following.Theorem 3.
(MMSE estimator for BLF). Letxk=xkTx˜kTT∈ℝ2n be a joint normal random vector, and x^k∈ℝ2n and Pk∈ℝ2n×2n are the Kalman estimate and block error covariance matrix (33). Then, the optimal MMSE estimator for the BLF uk=xkTAkx˜k has the following closed-form structure:(34)u^kopt=x^kTAkx˜^k+trAkPxx˜,k.Example 4.
(estimation of inner product and squared Euclidean distance). Using the bilinear estimator (34) with Ak=In, the MMSE estimator for the inner product uk=xkTx˜k takes the form(35)u^kopt=x^kTx˜^k+trPxx˜,k.
Next, calculate the optimal MMSE estimator for the squared Euclidean distance between two pointszk=d2xk,x˜k=xk−x˜k22 or zk=ηk22, where ηk=xk−x˜k. The Kalman estimate and error covariance of the difference ηk take the form(36)η^k=x^k−x˜^k,Pkη=Pxx,k+Px˜x˜,k−Pxx˜,k−Pxx˜,kT.
Applying the quadratic estimator (26) with Ak=In, we obtain the MMSE estimator for the squared Euclidean distance:(37)z^kopt=η^kTη^k+trPkη=x^k−x˜^kTx^k−x˜^k+trPkη.
The MMSE estimators for bilinear and quadratic forms are summarized in Table5.Table 5
MMSE estimators for QF, BLF, inner product, and squared norm.
TitleFormula for functionMMSE estimatorEquationQFz=xTAx,A=ATz^opt=x^TAx^+trAP(26)BLFu=xTBx˜,B=BTu^opt=x^TBx˜^+trBPxx˜(34)Inner productu=x,x˜=xTx˜u^opt=x^Tx˜^+trPxx˜(35)Euclidean distancez=x−x˜22z^opt=x^−x˜^Tx^−x˜^+trPη,Pη=Pxx+Px˜x˜−Pxx˜−Pxx˜T(37)


## 5.3. Practical Usefulness of Squared Euclidean Distance

In many practical problems, for example, finding the shortest distance from a point to a curve,minx,x˜∈ℳdx,x˜, or comparing a distance with a threshold value, dx,x˜≷ε, there is no need to calculate the original Euclidean distance dx,x˜=∑i=1nxi−x˜i2, but we just need to calculate its square due to the equivalence of the problems, minx,x˜∈ℳdx,x˜⇔minx,x˜∈ℳd2x,x˜ or dx,x˜≷ε⟺d2x,x˜≷ε2. In such situations, the optimal quadratic estimator (37) for the squared Euclidean distance, d2x,x˜=∑i=1nxi−x˜i2, can be successfully used.Example 5.
(deviation of normal and nominal trajectories). Suppose that the piecewise feedback control lawUk∗ depends on the difference between a normal xk and nominal xkn trajectories. For example, it is given by(38)Uk∗=1,dxk,xkn<D,−1,otherwise,where dxk,xkn is the Euclidean distance and D is the distance threshold (see Figure 9).
In view of the above, rewrite the control law in the equivalent form:(39)Uk∗=1,dxk,xkn<D2,−1,otherwise,where d2xk,xkn is the squared of the Euclidean distance and D2 is the new threshold.
Using the quadratic estimator (37) for the squared distance zk=d2xk,xkn, we obtain the MMSE estimator, z^kopt=x^k−x^knTx^k−x^kn+trPk, which can be used in the control (39):(40)Uk∗=1,z^kopt<D2,−1,otherwise.
In the next section, we discuss application of the linear, bilinear, and quadratic estimators (Theorems1–3) for estimation of composite nonlinear functions.Figure 9
Distance deviation between normal and nominal (desired) trajectories.


## 6. Suboptimal Estimator for Composite Nonlinear Functions

### 6.1. Definition of Composite Function

Consider a composite functionF depending on LF, QF, and BLF, such as(41)Fx=Fg1x,g2x,…,ghx,where the inside functions are defined as(42)gix=LF=cTx,x,c∈ℝn,QF=xTAx,x∈ℝn,A∈ℝn×n,BLF=xTBx˜,x,x˜∈ℝn,B∈ℝn×n.Example 6.
(composite and inside functions in object tracking). Letx∈ℝ6 be an object state vector consisting of the position px,py,pz and corresponding velocity vx,vy,vz components in the Cartesian coordinates x,y,z, i.e.,(43)x=x1x2x3x4x5x6T=pxpypzvxvyvzT.
In the spherical coordinates, we assume that a Doppler radar is located at the origin of the Cartesian coordinates, and it measures the following quantities obtained via nonlinear composite functionsFig1x,…,ghx of the state components depending on LF, QF, and BLF:(44)d=px2+py2+pz2=F1x,F1x=g1x,g1=x12+x22+x32,θ=tan−1pypx=F2x,F2x=tan−1g2xg3x,g2=x2,g3=x1,φ=pzpx2+py2=F3x,F3x=g4xg5x,g4=x3,g5=x12+x22,d˙=pxvx+pyvy+pzvzpx2+py2+pz2=F4x,F4x=g6xg1x,g6=x1x4+x2x5+x3x6,where d is the range (distance), θ is the bearing angle, φ is the elevation angle, and d˙ is the range rate.


### 6.2. Suboptimal Estimator for Composite Functions

Given the Kalman estimate and covariancex^,P, we estimate a quantity obtained via the composite function Fx=Fg1x,…,ghx. The idea of the algorithm is based on the optimal MMSE estimators for LF, QF, and BLF proposed in equations (10), (26), and (34), respectively. We have(45)For LFgix=cTx:g^ix^,P=cTx^;For QFgix=xTAx:g^ix^,P=x^TAx^+trAP;For BLFgix=xTBx˜:g^ix^,P=x^TBx˜^+trBPxx˜.Replacing the unknown inside functionsgix with the corresponding optimal estimates (45), we obtain the novel suboptimal estimator for the composite function z=Fg1,…,gh, i.e.,(46)z^comp=Fg^1xˆ,P,…,g^hxˆ,P.Example 7.
(estimation of cosine of angle). LetX=xTx˜TT∈ℝ2n be a joint normal state vector, and(47)X⌢=x^x˜^,P=PxxPxx˜Pxx˜TPx˜x˜are the Kalman estimate and block error covariance.
The cosine of angle between two vectorsx,x˜∈ℝn is equal to(48)cosθ=x,x˜x×x˜=x,x˜xTx×x˜Tx˜.
We observe the ratio (48) represents the composite function z=Fx,x˜ depending on the three inside functions g1=xTx, g2=x˜Tx˜, and g0=x˜,x˜:(49)z=cosθ=g0g1×g2.
The optimal MMSE estimators for the inside functionsg0,g1, and g2 are known. Using equation (45), we have(50)g^0=x^Tx˜^+trPxx˜,g^1=x^Tx^+trPxx,g^2=x˜^Tx˜^+trPx˜x˜.
Replacing the inside functionsgi with their estimates g^i, we get the suboptimal estimator for the cosine of angle:(51)z^com=g^0g^1g^2=x^Tx˜^+trPxyx^Tx^+trPxx×x˜^Tx˜^+trPx˜x˜.
Numerical example illustrates the applicability of the all estimators proposed in the paper.


## 6.1. Definition of Composite Function

Consider a composite functionF depending on LF, QF, and BLF, such as(41)Fx=Fg1x,g2x,…,ghx,where the inside functions are defined as(42)gix=LF=cTx,x,c∈ℝn,QF=xTAx,x∈ℝn,A∈ℝn×n,BLF=xTBx˜,x,x˜∈ℝn,B∈ℝn×n.Example 6.
(composite and inside functions in object tracking). Letx∈ℝ6 be an object state vector consisting of the position px,py,pz and corresponding velocity vx,vy,vz components in the Cartesian coordinates x,y,z, i.e.,(43)x=x1x2x3x4x5x6T=pxpypzvxvyvzT.
In the spherical coordinates, we assume that a Doppler radar is located at the origin of the Cartesian coordinates, and it measures the following quantities obtained via nonlinear composite functionsFig1x,…,ghx of the state components depending on LF, QF, and BLF:(44)d=px2+py2+pz2=F1x,F1x=g1x,g1=x12+x22+x32,θ=tan−1pypx=F2x,F2x=tan−1g2xg3x,g2=x2,g3=x1,φ=pzpx2+py2=F3x,F3x=g4xg5x,g4=x3,g5=x12+x22,d˙=pxvx+pyvy+pzvzpx2+py2+pz2=F4x,F4x=g6xg1x,g6=x1x4+x2x5+x3x6,where d is the range (distance), θ is the bearing angle, φ is the elevation angle, and d˙ is the range rate.


## 6.2. Suboptimal Estimator for Composite Functions

Given the Kalman estimate and covariancex^,P, we estimate a quantity obtained via the composite function Fx=Fg1x,…,ghx. The idea of the algorithm is based on the optimal MMSE estimators for LF, QF, and BLF proposed in equations (10), (26), and (34), respectively. We have(45)For LFgix=cTx:g^ix^,P=cTx^;For QFgix=xTAx:g^ix^,P=x^TAx^+trAP;For BLFgix=xTBx˜:g^ix^,P=x^TBx˜^+trBPxx˜.Replacing the unknown inside functionsgix with the corresponding optimal estimates (45), we obtain the novel suboptimal estimator for the composite function z=Fg1,…,gh, i.e.,(46)z^comp=Fg^1xˆ,P,…,g^hxˆ,P.Example 7.
(estimation of cosine of angle). LetX=xTx˜TT∈ℝ2n be a joint normal state vector, and(47)X⌢=x^x˜^,P=PxxPxx˜Pxx˜TPx˜x˜are the Kalman estimate and block error covariance.
The cosine of angle between two vectorsx,x˜∈ℝn is equal to(48)cosθ=x,x˜x×x˜=x,x˜xTx×x˜Tx˜.
We observe the ratio (48) represents the composite function z=Fx,x˜ depending on the three inside functions g1=xTx, g2=x˜Tx˜, and g0=x˜,x˜:(49)z=cosθ=g0g1×g2.
The optimal MMSE estimators for the inside functionsg0,g1, and g2 are known. Using equation (45), we have(50)g^0=x^Tx˜^+trPxx˜,g^1=x^Tx^+trPxx,g^2=x˜^Tx˜^+trPx˜x˜.
Replacing the inside functionsgi with their estimates g^i, we get the suboptimal estimator for the cosine of angle:(51)z^com=g^0g^1g^2=x^Tx˜^+trPxyx^Tx^+trPxx×x˜^Tx˜^+trPx˜x˜.
Numerical example illustrates the applicability of the all estimators proposed in the paper.


## 7. Numerical Example: Motion in a Plane

In this section, we estimate the range and the bearing angle in 2-D motion of an object. Because of the difficulties of getting analytical closed-form expressions for the optimal estimators for range and bearing, we apply the simple estimatorz^sim and the estimator based on the composite functions z^com. In addition, we are interested in the angle between the two state vectors xk−1 and xk at time instants tk−1 and tk, respectively, φk=def∠xk-1,xk.


### 7.1. Suboptimal Estimators for Range-Angle Response

The example of Section4.3.2 is considered again. Consider the 2-D models (22) and (23) describing motion of the two random points A1x1,k and A2x2,k. To calculate the range dk, tangent of the bearing angle θk, and cosine of the angle φk, we use the following formulas:(52)fxk=defdk=x1,k2+x2,k2,hxk=deftanθk=x2,kx1,k,gxk=defcosφk=xk−1,xkxk−1×xk=x1,k−1x1,k+x2,k−1x2,kx1,k−12+x2,k−12×x1,k2+x2,k2.The following estimators for the range-angle responses (52) are illustrated and compared:(1)
Simple estimator:(53)af^ksim=x^1,k2+x^2,k2,bh^ksim=x^2,kx^1,k,cg^ksim=x^1,k−1x^1,k+x^2,k−1x^2,kx^1,k−12+x^2,k−12×x^1,k2+x^2,k2.(2)
Estimator for composite functions:(54)af^kcom=x^1,k2+P11,k+x^2,k2+P22,k,bh^kcom=h^ksim,cg^kcom=x^1,k−1x^1,k+x^2,k−1x^2,k+Pk−1,k1+Pk−1,k2x^1,k−12+P11,k−1+x^2,k−12+P22,k−1×x^1,k2+P11,k+x^2,k2+P22,k.Note that the simple and composite estimatesh^ksim and h^kcom for a bearing angle coincide. In equation (54), Pk−1,k1=Ee1,k−1e1,k and Pk−1,k2=Ee2,k−1e2,k are the error cross-covariances satisfying the following recursive:(55)Pk−1,ki=1−Kk−1i1−KkiPk−2,k−1i,k≥2,P0,1i=1−Kkiσi2,σi2=Covxi,0,i=1,2.In equations (53)–(55), the values x^i,k,Kki, and Pii,k=Eei,k2 represent the Kalman estimate, KF gain, and variance of the error ei,k=xi,k−x^i,k, respectively.


### 7.2. Simulation Results

The simple and composite estimators were run with the same random noises for further comparison. The Monte Carlo simulation with 1000 runs was applied in calculation of the RMSEs for the rangedk, the bearing angle θk, and the angle between state vectors φk. Figures 10–12 show the range and angle estimates for the model parameters in equations (22) and (23), with m1=0.1,m2=−0.1, P0=I2, q1=0.2,q2=0.3, r1=0.05,r2=0.1, and r12=0. The following results about the relative performance of the above estimators can be made.Figure10(a) presents the range estimators f^ksim,f^kcom as well as the true range dk. Figure 10(b) shows the comparison of the RMSEs for the range estimators. Comparing R¯r^kcom and R¯r^ksim on the interval k∈1;20, we obtain the values 0.0296 and 0.0541, respectively. From Figures 10(a) and 10(b), the range estimator f^kcom has the better performance compared to the simple one f^ksim. This is due to the fact that the MMSE estimate z^kopt=x^1,k2+P11,k+x^2,k2+P22,k of the squared norm zk=xk2=x1,k2+x2,k2 contains the error variances P11,k and P22,k as additional terms. If the variances tend to zero, Pii,k⟶0, then the range estimators will converge, i.e., f^kcom≈f^ksim.(2)
Figure11 shows the true value of tangent of the bearing angle hxk=x2,k/x1,k and the corresponding simple (or composite) estimate, h^ksim=h^kcom=x^2,k/x^1,k. We observe the negligible difference between the true tangent value hxk and its simple estimate h^ksim. The average RMSE of the estimate over the time interval 1,20 is R¯h^sim=0.0672. It demonstrates reasonable accuracy of the estimator x^2,k/x^1,k for the unknown ratio (tangent of angle) x^2,k/x^1,k.(3)
Similar simulation procedures, as in (1) and (2), were used to check performance of the estimatorsg^ksim and g^kcom. The true cosine value gk is shown in Figure 12 for comparison with the estimated values.Figure 10
Comparison of composite and simple estimators for range. (a) True and estimated ranges. (b) RMSE for both estimators.
(a)(b)Figure 11
True tangent of bearing angle and its simple (composite) estimate.Figure 12
True cosine of angle and simple and composite estimates.For detailed consideration of the proposed estimators, we divide the whole time interval into two subintervalsI1=1;6 and I2=7;20, respectively. From Figure 12, we can observe that on the first subinterval, the estimate g^kcom is better than g^ksim, and on the second one, the difference between them is negligible. This is also confirmed by the values of R¯g^k presented in Table 6.Table 6
Comparisons of average RMSE for the simple and composite estimators.
R¯g^ksimR¯g^kcomI1=1;60.65190.4886I2=7;200.12440.1226Note that both estimatorsg^ksim and g^kcom are based on the MMSE estimators for a squared norm and inner product. Therefore, the difference between them becomes small if the KF error variances Pii,k are small (see (c) in equations (53) and (54)). In our case, the steady-state values of the variances are P11,k=0.0214 and P22,k=0.0101,k>8.


## 7.1. Suboptimal Estimators for Range-Angle Response

The example of Section4.3.2 is considered again. Consider the 2-D models (22) and (23) describing motion of the two random points A1x1,k and A2x2,k. To calculate the range dk, tangent of the bearing angle θk, and cosine of the angle φk, we use the following formulas:(52)fxk=defdk=x1,k2+x2,k2,hxk=deftanθk=x2,kx1,k,gxk=defcosφk=xk−1,xkxk−1×xk=x1,k−1x1,k+x2,k−1x2,kx1,k−12+x2,k−12×x1,k2+x2,k2.The following estimators for the range-angle responses (52) are illustrated and compared:(1)
Simple estimator:(53)af^ksim=x^1,k2+x^2,k2,bh^ksim=x^2,kx^1,k,cg^ksim=x^1,k−1x^1,k+x^2,k−1x^2,kx^1,k−12+x^2,k−12×x^1,k2+x^2,k2.(2)
Estimator for composite functions:(54)af^kcom=x^1,k2+P11,k+x^2,k2+P22,k,bh^kcom=h^ksim,cg^kcom=x^1,k−1x^1,k+x^2,k−1x^2,k+Pk−1,k1+Pk−1,k2x^1,k−12+P11,k−1+x^2,k−12+P22,k−1×x^1,k2+P11,k+x^2,k2+P22,k.Note that the simple and composite estimatesh^ksim and h^kcom for a bearing angle coincide. In equation (54), Pk−1,k1=Ee1,k−1e1,k and Pk−1,k2=Ee2,k−1e2,k are the error cross-covariances satisfying the following recursive:(55)Pk−1,ki=1−Kk−1i1−KkiPk−2,k−1i,k≥2,P0,1i=1−Kkiσi2,σi2=Covxi,0,i=1,2.In equations (53)–(55), the values x^i,k,Kki, and Pii,k=Eei,k2 represent the Kalman estimate, KF gain, and variance of the error ei,k=xi,k−x^i,k, respectively.


## 7.2. Simulation Results

The simple and composite estimators were run with the same random noises for further comparison. The Monte Carlo simulation with 1000 runs was applied in calculation of the RMSEs for the rangedk, the bearing angle θk, and the angle between state vectors φk. Figures 10–12 show the range and angle estimates for the model parameters in equations (22) and (23), with m1=0.1,m2=−0.1, P0=I2, q1=0.2,q2=0.3, r1=0.05,r2=0.1, and r12=0. The following results about the relative performance of the above estimators can be made.Figure10(a) presents the range estimators f^ksim,f^kcom as well as the true range dk. Figure 10(b) shows the comparison of the RMSEs for the range estimators. Comparing R¯r^kcom and R¯r^ksim on the interval k∈1;20, we obtain the values 0.0296 and 0.0541, respectively. From Figures 10(a) and 10(b), the range estimator f^kcom has the better performance compared to the simple one f^ksim. This is due to the fact that the MMSE estimate z^kopt=x^1,k2+P11,k+x^2,k2+P22,k of the squared norm zk=xk2=x1,k2+x2,k2 contains the error variances P11,k and P22,k as additional terms. If the variances tend to zero, Pii,k⟶0, then the range estimators will converge, i.e., f^kcom≈f^ksim.(2)
Figure11 shows the true value of tangent of the bearing angle hxk=x2,k/x1,k and the corresponding simple (or composite) estimate, h^ksim=h^kcom=x^2,k/x^1,k. We observe the negligible difference between the true tangent value hxk and its simple estimate h^ksim. The average RMSE of the estimate over the time interval 1,20 is R¯h^sim=0.0672. It demonstrates reasonable accuracy of the estimator x^2,k/x^1,k for the unknown ratio (tangent of angle) x^2,k/x^1,k.(3)
Similar simulation procedures, as in (1) and (2), were used to check performance of the estimatorsg^ksim and g^kcom. The true cosine value gk is shown in Figure 12 for comparison with the estimated values.Figure 10
Comparison of composite and simple estimators for range. (a) True and estimated ranges. (b) RMSE for both estimators.
(a)(b)Figure 11
True tangent of bearing angle and its simple (composite) estimate.Figure 12
True cosine of angle and simple and composite estimates.For detailed consideration of the proposed estimators, we divide the whole time interval into two subintervalsI1=1;6 and I2=7;20, respectively. From Figure 12, we can observe that on the first subinterval, the estimate g^kcom is better than g^ksim, and on the second one, the difference between them is negligible. This is also confirmed by the values of R¯g^k presented in Table 6.Table 6
Comparisons of average RMSE for the simple and composite estimators.
R¯g^ksimR¯g^kcomI1=1;60.65190.4886I2=7;200.12440.1226Note that both estimatorsg^ksim and g^kcom are based on the MMSE estimators for a squared norm and inner product. Therefore, the difference between them becomes small if the KF error variances Pii,k are small (see (c) in equations (53) and (54)). In our case, the steady-state values of the variances are P11,k=0.0214 and P22,k=0.0101,k>8.


## 8. Conclusion

In this paper, we propose a novel MMSE approach for the estimation of distance metrics under the Kalman filtering framework. The main contributions of the paper are listed in the following.Firstly, an optimal two-stage MMSE estimator for an arbitrary nonlinear function of a state vector is proposed. The distance metric is an important practical case of such nonlinearities, detailed study of which is given in the paper. Implementation of the MMSE estimator is reduced to calculation of the multivariate Gaussian integral. To avoid the difficulties associated with its calculation, the concept of a closed-form estimator depending on the Kalman filter statistics is introduced. We establish relations between the Euclidean metrics and the closed-form estimator, which lead to simple compact formulas for the real-life distances between points presented in Table2.Secondly, an important class of bilinear and quadratic estimators is comprehensively studied. These estimators are applied to a square of norm, Euclidean distance, and inner product. Table5 summarizes the results. Moreover, an effective low-complexity suboptimal estimator for nonlinear composite functions is developed using the MMSE bilinear and quadratic estimators. As shown in Section 6.1, radar tracking range-angle responses are described by the composite functions.Simulation and experimental results show that the proposed estimators perform significantly better than the existing suboptimal distance or angle estimators such as a simple estimator defined in the paper. The low-complexity estimator developed in Section6.1 is quite promising for radar data processing. Also, the numerical results confirm the fact that the more accurate the Kalman estimate of a state vector, the more accurately we can obtain the range and angle estimates.



---

*Source: 9141735-2020-08-20.xml*