Random Weighting, Strong Tracking, and Unscented Kalman Filter for Soft Tissue Characterization

Abstract

This paper presents a new nonlinear filtering method based on the Hunt-Crossley model for online nonlinear soft tissue characterization. This method overcomes the problem of performance degradation in the unscented Kalman filter due to contact model error. It adopts the concept of Mahalanobis distance to identify contact model error, and further incorporates a scaling factor in predicted state covariance to compensate identified model error. This scaling factor is determined according to the principle of innovation orthogonality to avoid the cumbersome computation of Jacobian matrix, where the random weighting concept is adopted to improve the estimation accuracy of innovation covariance. A master-slave robotic indentation system is developed to validate the performance of the proposed method. Simulation and experimental results as well as comparison analyses demonstrate that the efficacy of the proposed method for online characterization of soft tissue parameters in the presence of contact model error.

1. Introduction

Soft tissue properties are important for robotic-assisted minimally invasive surgery to achieve realistic haptic feedback and stable robotic control. However, soft tissue properties are dynamically changing depending on tissue layers, organs, patients, and physiological conditions. Accordingly, it is necessary to online characterize dynamic soft tissue properties from tool-tissue interaction measurements for robotic-assisted minimally invasive surgery [1,2,3].

The identification of soft tissue properties relies on the knowledge of the contact interaction between surgical tool and soft tissues. Due to the computational efficiency, various contact models have been used to describe the mechanical contact with soft tissues. The simple one is the linear elastic model, which fails to represent the complex behaviour of tool–tissue interaction [4]. The Maxwell (MW) model is constructed by a spring and damper in serial form [5], while the Kelvin–Voigt (KV) model a spring and damper in parallel form [6]. The Kelvin–Boltzmann (KB) model adds a spring in serial form to the Kelvin–Voigt model [7]. However, these three are linear spring-damper models and involves unnatural forces at start and end points of the contact [8]. The Hunt-Crossley (H-C) model is nonlinear and solves the unnatural force problem involved in the linear K-V model [8,9]. Its nonlinearity is also suitable for characterization of nonlinear soft tissue behaviours for robotic-assisted minimally invasive surgery. However, the use of the nonlinear H-C model for online soft tissue characterization requires an online nonlinear estimation algorithm, which is more complicated than an online linear estimation algorithm. Thus, there has been limited research on using the nonlinear H-C model for soft tissue characterization. Although finite element model (FEM) [10] provides an accurate description of the force interaction between surgical tool and soft tissues using continuum mechanics, it is difficult to achieve the real-time performance due to expensive computations [3,11].

In addition to a dynamic contact model, an online estimation algorithm is also required for real-time soft tissue characterization. The recursive least square (RLS) is an online estimation algorithm for soft tissue characterization [8,9]. However, its performance is sensitive to initial conditions [9]. The Kalman filter (KF) is an improvement to RLS. It can provide estimations in accuracy of minimum mean-square error. However, both RLS and KF are a linear estimation algorithm, unsuitable for the use with the nonlinear H-C model for soft tissue characterization [8].

Various nonlinear versions of KF were reported by extending the concept of KF to nonlinear systems. The extended Kalman filter (EKF) is a commonly used nonlinear estimation algorithm. However, due to the involvement of model linearization, it can only achieve first-order accuracy [12,13]. EKF also requires the cumbersome computation of Jacobian matrix at each time point. The unscented Kalman filter (UKF) improves EKF by approximating state mean and covariance via unscented transformation [10,14]. It can achieve third-order accuracy. Further, it does not require the cumbersome calculation of Jacobian matrix. However, UKF requires an accurate system model. In robotic-assisted minimally invasive surgery, the system model always involve uncertainties such as inappropriate initial conditions, modelling error due to model simplification for the purpose of computational efficiency, unexpected system noises and stochastic drifts, leading to the deteriorated UKF solution [15,16]. Currently, there has been limited research focusing on the use of UKF for nonlinear soft tissue characterization. Xi et al. reported a method by combining UKF with a FEM to estimate myocardial material parameters [10]. However, the FEM is under the quasi-static assumption, which does not hold for a dynamic contact system. Further, the UKF estimation performance is degraded due to the reduced size for state vector’s error covariance. Recently, the authors also studied a method by combing UKF with the H-C model for soft tissue characterization [17]. However, the UKF problem in requirement of accurate system model was not addressed.

Adaptive filtering is a strategy to handle the disturbance of system model error on state estimation. It has been used with UKF, leading to the adaptive UKF to inhibit the influence of system model error on the filtering solution [18]. The innovation based adaptive estimation and residual based adaptive estimation are two typical methods for adaptive filtering [19], where the innovation or residual at present time point is adaptively estimated from all historical innovations or residuals within a small time window [20]. However, due to the use of equal weighting for all historical innovations within the time window, each historical innovation or residual within the time window equally contributes to the current innovation or residual, leading to limited estimation accuracy. The random weighting method overcomes this problem by taking into account different precision levels of historical innovations or residuals in the current innovation or residual via random weights, leading to improved estimation accuracy [15,21].

The strong tracking (ST) is a relatively new concept in adaptive filtering. It incorporates a scaling factor into predicted state covariance to compensate system model error [22,23,24]. In addition to the strong robustness against system model error, ST is also able to online track system state. However, ST requires the cumbersome evaluations of Jacobian matrix to calculate the scaling factor, leading to an extra computational burden.

This paper presents a new nonlinear filtering method based on the nonlinear H-C contact model by combining the concepts of ST and random weighting into UKF for online soft tissue characterization. This method adopts the ST concept to address the UKF problem of performance degradation due to contact model error. It identifies contact model error using the Mahalanobis distance. Subsequently, a scaling factor is introduced into predicted state covariance to account for identified contact model error. To avoid the cumbersome calculation of Jacobian matrix, this scaling factor is determined according to the orthogonality principle, where the random weighting concept is adopted to enhance the estimation accuracy of innovation covariance. Simulations, practical experiments, and comparison analysis with UKF have been conducted to comprehensively evaluate the performance of the proposed method.

2. Nonlinear Hunt-Crossley Contact Model

The nonlinear H-C contact model describes the dynamics of the contact between surgical tool and soft tissues via the following nonlinear equation [25]:

$$F=K{d}^n+B{d}^n{\dot{d}}^p$$

(1)

where F, K, B, $d$, $\dot{d}$, n, and p are the contact force, stiffness coefficient, damping coefficient, displacement, displacement velocity, power of displacement, and power of displacement velocity, respectively.

Define the state vector as:

$${\mathit{x}}_k=\left[d{\left(t\right)}_k,\dot{d}{\left(t\right)}_k,F_k,K_k,B_k,n_k,p_k\right].$$

(2)

Based on Equation (1), the system state equation can be formulated as:

$${\mathit{x}}_k=f\left({\mathit{x}}_{k-1}\right)+{\mathit{q}}_{k-1}=\left\{\begin{array}{c}{d}_{k-1}+{\dot{d}}_{k-1}\times \Delta t_{k-1}\\ {\dot{d}}_{k-1}\\ K_{k-1}{d}_{k-1}^{n_{k-1}}+B_{k-1}{d}_{k-1}^{n_{k-1}}{\dot{d}}_{k-1}^{p_{k-1}}\\ K_{k-1}\\ B_{k-1}\\ n_{k-1}\\ p_{k-1}\end{array}\right\}+{\mathit{q}}_{k-1}$$

(3)

where ${\mathit{x}}_k=\left[d{\left(t\right)}_k,\dot{d}{\left(t\right)}_k,F_k,K_k,B_k,n_k,p_k\right]$ is the system state at time point $t_k$, $f(·)$ is the system function, and ${\mathit{q}}_k~\left(0,{\mathit{Q}}_k\right)$ is a white Gaussian noise with zero mean and covariance ${\mathit{Q}}_k$.

The measurement equation is defined as:

$${\mathit{y}}_k=h\left({\mathit{x}}_k\right)+{\mathit{r}}_k=\left(\begin{array}{c}{d}_k\\ F_k\end{array}\right)+{\mathit{r}}_k$$

(4)

where ${\mathit{y}}_k$ is the measurement at time point $t_k$, h$(·)$ is the measurement function which describes the relation between the measurement and system state, and ${\mathit{r}}_k~\left(0,{\mathit{R}}_{\mathrm{k}}\right)$ is a white Gaussian noise with zero mean and covariance ${\mathit{R}}_{\mathrm{k}}$.

It should be noted that in robotic-assisted minimally invasive surgery, the accuracy of the measurement model can be guaranteed, since force data can be obtained from a high-accurate force sensor and displacement data can be obtained from precise robotic encoder.

3. Analysis of Unscented Kalman Filter

The conventional UKF procedure can be described as follows:

Step 1. Initialization:

$${\widehat{\mathit{x}}}_0=E\left({\mathit{x}}_0\right)$$

(5)

$${\widehat{\mathit{P}}}_0=E\left[\left({\mathit{x}}_0-{\widehat{\mathit{x}}}_0\right){\left({\mathit{x}}_0-{\widehat{\mathit{x}}}_0\right)}^T\right].$$

(6)

Step 2. Time update with unscented transformation:

Select sigma points:

$$\begin{gathered} {\widehat{\mathit{x}}}_{k-1}^{\left(i\right)}={\widehat{\mathit{x}}}_{k-1}+{\widehat{\mathit{x}}}^{\left(i\right)}\left(i=1,\cdots ,2N\right) \\ {\widehat{\mathit{x}}}^{\left(i\right)}={\left(\sqrt[]{\left(N+\lambda \right){\widehat{\mathit{P}}}_{k-1}}\right)}_i^T\left(i=1,\cdots ,N\right) \\ {\widehat{\mathit{x}}}^{\left(N+i\right)}=-{\left(\sqrt[]{\left(N+\lambda \right){\widehat{\mathit{P}}}_{k-1}}\right)}_i^T\left(i=1,\cdots ,N\right) \end{gathered}$$

(7)

where $N$ is the dimension of state vector $\mathit{x}$, the parameter $\lambda$ is defined as $\lambda ={\alpha }^2\left(N+k\right)-N$ with constant $\alpha$, and ${\widehat{\mathit{P}}}_{k-1}$ is the estimated state covariance at time point $t_{k-1}$ (k = 1, 2, …).

Calculate predicted state vector ${\overline{\mathit{x}}}_k$ based on the selected sigma points:

$$\begin{gathered} {\overline{\mathit{x}}}_k^{\left(i\right)}=f({\widehat{\mathit{x}}}_{k-1}^{\left(i\right)}) \\ {\overline{\mathit{x}}}_k=\frac{1}{2N}{\displaystyle \sum }_{i=1}^{2N}{w}_i^m({\overline{\mathit{x}}}_k^{\left(i\right)}). \end{gathered}$$

(8)

Calculate predicted state covariance ${\overline{\mathit{P}}}_k$:

$$\begin{gathered} {\overline{\mathit{P}}}_k={\displaystyle \sum }_{i=1}^{2N}{w}_i^c\left({\overline{\mathit{x}}}_k^{\left(i\right)}-{\overline{\mathit{x}}}_k\right){\left({\overline{\mathit{x}}}_k^{\left(i\right)}-{\overline{\mathit{x}}}_k\right)}^T+{\mathit{Q}}_k \\ \begin{array}{l}{w}_0^m=\frac{\lambda }{n+\lambda }\hfill \\ w_0^c=\frac{\lambda }{n+\lambda }+\left(1-{\alpha }^2+\beta \right)\hfill \\ w_i^m=w_i^c=\frac{1}{2\left(N+\lambda \right)}i=1,\dots ,2N\hfill \end{array} \end{gathered}$$

(9)

where $w_i^m$ and $w_i^c$ are the mean and covariance weights, $N$ is the dimension of state vector $x$, and $\alpha$ and $\beta$ are constants.

Step 3. Measurement update:

Calculate predicted measurement ${\overline{\mathit{y}}}_k$:

$$\begin{gathered} {\overline{\mathit{y}}}_k^{\left(i\right)}=h({\overline{\mathit{x}}}_k^{\left(i\right)}) \\ {\overline{\mathit{y}}}_k=\frac{1}{2N}{\displaystyle \sum }_{i=1}^{2N}{w}_i^m({\overline{\mathit{y}}}_k^{\left(i\right)}) \end{gathered}$$

(10)

Calculate predicted measurement covariance ${\mathit{P}}_{{\overline{\mathit{y}}}_k}$:

$${\mathit{P}}_{{\overline{\mathit{y}}}_k}={\displaystyle \sum }_{i=1}^{2N}{w}_i^c\left({\overline{\mathit{y}}}_k^{\left(i\right)}-{\overline{\mathit{y}}}_k\right){\left({\overline{\mathit{y}}}_k^{\left(i\right)}-{\overline{\mathit{y}}}_k\right)}^T+{\mathit{R}}_k$$

(11)

Calculate the cross covariance between ${\overline{\mathit{x}}}_k$ and ${\overline{\mathit{y}}}_k$:

$${\mathit{P}}_{{\overline{\mathit{x}}}_k{\overline{\mathit{y}}}_k}={\displaystyle \sum }_{i=1}^{2N}{w}_i^c\left({\overline{\mathit{x}}}_k^{\left(i\right)}-{\overline{\mathit{x}}}_k\right){\left({\overline{\mathit{y}}}_k^{\left(i\right)}-{\overline{\mathit{y}}}_k\right)}^T$$

(12)

Calculate the Kalman gain:

$${\mathit{K}}_k={\mathit{P}}_{{\overline{\mathit{x}}}_k{\overline{\mathit{y}}}_k}{\mathit{P}}_{{\overline{\mathit{y}}}_k}^{-1}$$

(13)

Update the estimated state and associated covariance:

$${\widehat{\mathit{x}}}_k={\overline{\mathit{x}}}_k+{\mathit{K}}_k{\mathit{Z}}_k^I$$

(14)

$${\widehat{\mathit{P}}}_k={\overline{\mathit{P}}}_k-{\mathit{K}}_k{\mathit{P}}_{{\overline{\mathit{y}}}_k}{\mathit{K}}_k^T$$

(15)

where ${\mathit{Z}}_k^I={\mathit{y}}_k-{\overline{\mathit{y}}}_k$ is called the innovation vector.

Now let us analyse the effect of contact model error on the UKF performance. The predicted state in Equation (8) can be rewritten as:

$${\overline{\mathit{x}}}_k=\frac{1}{2N}{\displaystyle \sum }_{i=1}^{2N}{\mathit{w}}_i^m\left(f\left({\overline{\mathit{x}}}_{k-1}^{\left(i\right)}\right)\right).$$

(16)

Since the contact state Equation (3) involves error $f_e(·)$, state prediction error ${\check{\mathit{x}}}_k$ can be represented as:

$${\check{\mathit{x}}}_k=\frac{1}{2N}{\displaystyle \sum }_{i=1}^{2N}{\mathit{w}}_i^m{f}_e\left({\overline{\mathit{x}}}_{k-1}^{\left(i\right)}\right).$$

(17)

The predicted state in the presence of model error $f_e(·)$ can be represented as:

$$\check{\overline{\mathit{x}}}={\overline{\mathit{x}}}_k+{\check{\mathit{x}}}_k=\frac{1}{2N}{\displaystyle \sum }_{i=1}^{2N}{\mathit{w}}_i^m\left(f\left({\overline{\mathit{x}}}_{k-1}^{\left(i\right)}\right)\right)+\frac{1}{2N}{\displaystyle \sum }_{i=1}^{2N}{\mathit{w}}_i^m{f}_e\left({\overline{\mathit{x}}}_{k-1}^{\left(i\right)}\right).$$

(18)

Using unscented transformation, the predicted state covariance in the presence of model error $f_e(·)$ can be calculated as:

$${\check{\overline{\mathit{P}}}}_k={\displaystyle \sum }_{i=1}^{2N}{\mathit{w}}_i^c\left({\overline{\mathit{x}}}_k^{\left(i\right)}-{\overline{\mathit{x}}}_k+{\check{\mathit{x}}}_k^{\left(i\right)}-{\check{\mathit{x}}}_k\right){\left({\overline{\mathit{x}}}_k^{\left(i\right)}-{\overline{\mathit{x}}}_k+{\check{\mathit{x}}}_k^{\left(i\right)}-{\check{\mathit{x}}}_k\right)}^T+{\mathit{Q}}_k.$$

(19)

where ${\check{\mathit{x}}}_k^{\left(i\right)}$ and ${\overline{\mathit{x}}}_k^{\left(i\right)}$ are the sigma points selected from ${\check{\mathit{x}}}_k$ and ${\overline{\mathit{x}}}_k$, respectively.

Denote:

$$\begin{gathered} {\overline{\mathit{X}}}_k^{\left(i\right)}={\overline{\mathit{x}}}_k^{\left(i\right)}-{\overline{\mathit{x}}}_k \\ {\check{\mathit{X}}}_k^{\left(i\right)}={\check{\mathit{x}}}_k^{\left(i\right)}-{\check{\mathit{x}}}_k. \end{gathered}$$

(20)

Thus, Equation (19) can be further written as:

$$\begin{gathered} {\check{\overline{\mathit{P}}}}_k={\displaystyle \sum }_{i=1}^{2N}{\mathit{w}}_i^c\left({\overline{\mathit{X}}}_k^{\left(i\right)}+{\check{\mathit{X}}}_k^{\left(i\right)}\right){\left({\overline{\mathit{X}}}_k^{\left(i\right)}+{\check{\mathit{X}}}_k^{\left(i\right)}\right)}^T+{\mathit{Q}}_k \\ ={\displaystyle \sum }_{i=1}^{2N}{\mathit{w}}_i^c\left({\overline{\mathit{X}}}_k^{\left(i\right)}{\overline{\mathit{X}}}_k^{\left(i\right)}{}^T+{\overline{\mathit{X}}}_k^{\left(i\right)}{\check{\mathit{X}}}_k^{\left(i\right)}+{\check{\mathit{X}}}_k^{\left(i\right)}{\overline{\mathit{X}}}_k^{\left(i\right)}{}^T+{\check{\mathit{X}}}_k^{\left(i\right)}{\check{\mathit{X}}}_k^{\left(i\right)}{}^T\right)+{\mathit{Q}}_k. \end{gathered}$$

(21)

Define the error ${\check{\mathit{P}}}_k$ of predicted state covariance:

$${\check{\mathit{P}}}_k={\displaystyle \sum }_{i=1}^{2N}{\mathit{w}}_i^c\left({\overline{\mathit{X}}}_k^{\left(i\right)}{\check{\mathit{X}}}_k^{\left(i\right)}+{\check{\mathit{X}}}_k^{\left(i\right)}{\overline{\mathit{X}}}_k^{\left(i\right)}{}^T+{\check{\mathit{X}}}_k^{\left(i\right)}{\check{\mathit{X}}}_k^{\left(i\right)}{}^T\right).$$

(22)

Substituting Equation (20) into Equation (21) yields:

$${\overline{\mathit{P}}}_k={\displaystyle \sum }_{i=1}^{2N}{\mathit{w}}_i^c\left({\overline{\mathit{X}}}_k^{\left(i\right)}{\overline{\mathit{X}}}_k^{\left(i\right)}{}^T\right)+{\mathit{Q}}_k.$$

(23)

Considering Equations (22) and (23), Equation (21) can be further written as:

$$\begin{array}{l}{\check{\overline{\mathit{P}}}}_k={\displaystyle {\displaystyle \sum }_{i=1}^{2N}}{\mathit{w}}_i^c\left({\overline{\mathit{X}}}_k^{\left(i\right)}{\overline{\mathit{X}}}_k^{\left(i\right)}{}^T\right)+{\mathit{Q}}_k+{\displaystyle {\displaystyle \sum }_{i=1}^{2N}}{\mathit{w}}_i^c\left({\overline{\mathit{X}}}_k^{\left(i\right)}{\check{\mathit{X}}}_k^{\left(i\right)}+{\check{\mathit{X}}}_k^{\left(i\right)}{\overline{\mathit{X}}}_k^{\left(i\right)}{}^T+{\check{\mathit{X}}}_k^{\left(i\right)}{\check{\mathit{X}}}_k^{\left(i\right)}{}^T\right)\\ ={\overline{\mathit{P}}}_k+{\check{\mathit{P}}}_k.\end{array}$$

(24)

It can be seen from Equation (24) that model error $f_e(·)$ causes predicted state covariance’s error ${\check{\mathit{P}}}_k$, leading to the inaccurate Kalman gain. Therefore, the state estimate will be degraded when the contact model involves error.

4. Random Weighting Strong Tracking Unscented Kalman Filter

This paper presents a random weighting strong tracking unscented Kalman filter (RWSTUKF) to address the UKF problem of performance degradation in the presence of model error for nonlinear soft tissue characterization. This method corrects predicted state covariance using the ST concept to restrain the disturbance of model error on state estimation, and also adopts the random weighting concept to improve the estimation accuracy of innovation covariance.

4.1. Correction of Predicted State Covariance

As analysed above, the predicted state covariance has the deviation ${\check{\mathit{P}}}_k$ due to model error $f_e(·)$. Using the deviation to correct the state covariance described by Equation (9) yields:

$${\overline{\mathit{P}}}_k^{*}={\displaystyle \sum }_{i=1}^{2N}{w}_i^c\left({\overline{\mathit{x}}}_k^{\left(i\right)}-{\overline{\mathit{x}}}_k\right){\left({\overline{\mathit{x}}}_k^{\left(i\right)}-{\overline{\mathit{x}}}_k\right)}^T+{\mathit{Q}}_k+{\check{\mathit{P}}}_k$$

(25)

where ${\overline{\mathit{P}}}_k^{*}$ denotes the corrected predicted state covariance.

Equation (25) can be further written as:

$$\begin{array}{l}{\overline{\mathit{P}}}_k^{*}={\gamma }_k\left({\displaystyle {\displaystyle \sum }_{i=1}^{2N}}{w}_i^c\left({\overline{\mathit{x}}}_k^{\left(i\right)}-{\overline{\mathit{x}}}_k\right){\left({\overline{\mathit{x}}}_k^{\left(i\right)}-{\overline{\mathit{x}}}_k\right)}^T+{\mathit{Q}}_k\right)\\ ={\gamma }_k{\overline{\mathit{P}}}_k\end{array}$$

(26)

where ${\gamma }_k$ is called the scaling factor, which is defined as:

$${\gamma }_k\mathit{I}=\mathit{I}+\frac{{\check{\mathit{P}}}_k}{{{\displaystyle \sum }}_{i=1}^{2N}{w}_i^c\left({\overline{\mathit{x}}}_k^{\left(i\right)}-{\overline{\mathit{x}}}_k\right){\left({\overline{\mathit{x}}}_k^{\left(i\right)}-{\overline{\mathit{x}}}_k\right)}^T+{\mathit{Q}}_k}$$

(27)

where $\mathit{I}$ is a unit matrix.

If we know the deviation ${\check{\mathit{P}}}_k$ in Equation (27) we can determine the scaling factor and calculate the corrected predicted state covariance ${\overline{\mathit{P}}}_k^{*}$ directly. However, the deviation ${\check{\mathit{P}}}_k$ is defined by the state prediction error ${\check{\mathit{x}}}_k={\overline{\mathit{x}}}_k-{\mathit{x}}_k$. Since the true state ${\mathit{x}}_k$ is generally unknown, it is difficult to directly calculate the deviation ${\check{\mathit{P}}}_k$. In order to solve this problem, this paper extracts all useful information in the innovation sequence via the orthogonality principle [26] to determine the scaling factor.

Theorem 1.

Under the condition of innovation orthogonality, i.e.:

$${\mathit{B}}_{j,k}=\mathit{E}\left[{\mathit{Z}}_k^I{}^T·{\mathit{Z}}_{k+j}^I\right]=0,j=1,2,\cdots .$$

(28)

${\gamma }_k$can be determined as:

$${\gamma }_k=\frac{tr\left({{\displaystyle \sum }}_{j=1}^M{v}_j{\mathit{Z}}_{k-j}^I{\mathit{Z}}_{k-j}^I{}^T\right)-tr\left({\mathit{R}}_k\right)}{tr\left({\mathit{H}}_k{\overline{\mathit{P}}}_k{\mathit{H}}_k{}^T\right)}$$

(29)

where${\mathit{H}}_k$=${\frac{\partial h\left(\mathit{x}\right)}{\partial \mathit{x}}|}_{\mathit{x}={\overline{\mathit{x}}}_k}$, $tr(·)$denotes the trace of a matrix, M is the window size, and$v_j$is the random weighting factor which meets the condition${\displaystyle \sum }_{j=1}^m{v}_j=1$.

Proof. 

Define the estimation error as:

$${\widehat{\mathit{e}}}_k={\mathit{x}}_k-{\widehat{\mathit{x}}}_k.$$

(30)

Define the prediction error as:

$${\overline{\mathit{e}}}_k={\mathit{x}}_k-{\overline{\mathit{x}}}_k.$$

(31)

Substituting Equations (3) and (8) into Equation (31) and expanding $f(·)$ by a Taylor series about ${\widehat{\mathit{x}}}_{k-1}$, the prediction error becomes:

$${\overline{\mathit{e}}}_k={\mathit{F}}_k{\widehat{\mathit{e}}}_{k-1}+{\mathit{q}}_k$$

(32)

where ${\mathit{F}}_k$ = ${\frac{\partial f\left(\mathit{x}\right)}{\partial \mathit{x}}|}_{\mathit{x}={\widehat{\mathit{x}}}_{k-1}}$.

By considering model error $f_e(·),$ Equation (32) becomes:

$${\overline{\mathit{e}}}_k=\left({\mathit{F}}_k+{\mathit{F}}_k^e\right){\widehat{\mathit{e}}}_{k-1}+{\mathit{q}}_k$$

(33)

where ${\mathit{F}}_k^e$ = ${\frac{\partial f_e\left(\mathit{x}\right)}{\partial \mathit{x}}|}_{\mathit{x}={\widehat{\mathit{x}}}_{k-1}}$.

Similar to Equation(32), innovation vector ${\mathit{Z}}_k^I$ can be represented as:

$${\mathit{Z}}_k^I={\mathit{H}}_k{\overline{\mathit{e}}}_k+{\mathit{r}}_k$$

(34)

where ${\mathit{H}}_k$ = ${\frac{\partial h\left(\mathit{x}\right)}{\partial \mathit{x}}|}_{\mathit{x}={\overline{\mathit{x}}}_k}$.

${\overline{\mathit{P}}}_k$ is defined by:

$${\overline{\mathit{P}}}_k=E\left[\left({\mathit{x}}_k-{\overline{\mathit{x}}}_k\right){\left({\mathit{x}}_k-{\overline{\mathit{x}}}_k\right)}^T\right].$$

(35)

${\mathit{P}}_{{\overline{\mathit{x}}}_k{\overline{\mathit{y}}}_k}$ is defined by:

$${\mathit{P}}_{{\overline{\mathit{x}}}_k{\overline{\mathit{y}}}_k}=E\left[\left({\mathit{x}}_k-{\overline{\mathit{x}}}_k\right){\left({\mathit{y}}_k-{\overline{\mathit{y}}}_k\right)}^T\right]=E\left[\left({\mathit{x}}_k-{\overline{\mathit{x}}}_k\right){\mathit{Z}}_k^I{}^T\right].$$

(36)

Substituting (34) into (36) and considering (35) and $\mathit{E}\left({\mathit{r}}_k\right)=0$, we have:

$$\begin{array}{l}{\mathit{P}}_{{\overline{\mathit{x}}}_k{\overline{\mathit{y}}}_k}=E\left[\left({\mathit{x}}_k-{\overline{\mathit{x}}}_k\right){\left({\mathit{H}}_k\left({\mathit{x}}_k-{\overline{\mathit{x}}}_k\right)+{\mathit{r}}_k\right)}^T\right]\\ =E\left[\left({\mathit{x}}_k-{\overline{\mathit{x}}}_k\right){\left({\mathit{x}}_k-{\overline{\mathit{x}}}_k\right)}^T{\mathit{H}}_k{}^T+{\mathit{r}}_k{}^T\right]\\ ={\overline{\mathit{P}}}_k{\mathit{H}}_k{}^T.\end{array}$$

(37)

Substituting (33) into (34) yields:

$${\mathit{Z}}_k^I={\mathit{H}}_k\left[\left({\mathit{F}}_k+{\mathit{F}}_k^e\right){\widehat{\mathit{e}}}_{k-1}+{\mathit{q}}_k\right]+{\mathit{r}}_k.$$

(38)

Substituting (38) into (28) leads to:

$$\begin{gathered} \begin{array}{l}{\mathit{B}}_{j,k}=E\left\{\left[{\mathit{H}}_{k+j}\left(\left({\mathit{F}}_{k+j}+{\mathit{F}}_{k+j}^e\right){\widehat{\mathit{e}}}_{k+j-1}+{\mathit{q}}_{k+j}\right)+{\mathit{r}}_{k+j}\right] \\ \times {\left[{\mathit{H}}_k\left(\left({\mathit{F}}_k+{\mathit{F}}_k^e\right){\widehat{\mathit{e}}}_{k-1}+{\mathit{q}}_k\right)+{\mathit{r}}_k\right]}^T\right\}\\ =E\left\{\left[{\mathit{H}}_{k+j}\left({\mathit{F}}_{k+j}+{\mathit{F}}_{k+j}^e\right)\left({\mathit{x}}_{k+j-1}-{\overline{\mathit{x}}}_{k+j-1}-{\mathit{K}}_{k+j-1}\left({\mathit{y}}_{k+j-1}-{\overline{\mathit{y}}}_{k+j-1}\right)\right)\right] \\ \times {\left[{\mathit{H}}_k\left(\left({\mathit{F}}_k+{\mathit{F}}_k^e\right){\widehat{\mathit{e}}}_{k-1}+{\mathit{q}}_k\right)+{\mathit{r}}_k\right]}^T\right\}\\ =E\left\{\left[{\mathit{H}}_{k+j}\left({\mathit{F}}_{k+j}+{\mathit{F}}_{k+j}^e\right)\left(\left({\mathit{F}}_{k+j-1}+{\mathit{F}}_{k+j-1}^e\right){\widehat{\mathit{e}}}_{k+j-2}-{\mathit{K}}_{k+j-1}\left({\mathit{H}}_k\left[\left({\mathit{F}}_k+{\mathit{F}}_k^e\right){\widehat{\mathit{e}}}_{k+j-2}\right]\right)\right)\right] \\ \times {\left[{\mathit{H}}_k\left(\left({\mathit{F}}_k+{\mathit{F}}_k^e\right){\widehat{\mathit{e}}}_{k-1}+{\mathit{q}}_k\right)+{\mathit{r}}_k\right]}^T\right\}\\ =E\left\{\left[{\mathit{H}}_{k+j}\left({\mathit{F}}_{k+j}+{\mathit{F}}_{k+j}^e\right)\left(I-{\mathit{K}}_{k+j-1}{\mathit{H}}_k\left({\mathit{F}}_k+{\mathit{F}}_k^e\right){\widehat{\mathit{e}}}_{k+j-2}\right)\right] \\ \times {\left[{\mathit{H}}_k\left(\left({\mathit{F}}_k+{\mathit{F}}_k^e\right){\widehat{\mathit{e}}}_{k-1}+{\mathit{q}}_k\right)+{\mathit{r}}_k\right]}^T\right\}\\ ={\mathit{H}}_{k+j}\left({\mathit{F}}_{k+j}+{\mathit{F}}_{k+j}^e\right)\times \left({\displaystyle {\displaystyle \prod }_{i=k+1}^{k+j-1}}\left(I-{\mathit{K}}_i{\mathit{H}}_i\right)\left({\mathit{F}}_i+{\mathit{F}}_i^e\right)\right)\times \left({\overline{\mathit{P}}}_{{\overline{\mathit{x}}}_k{\overline{\mathit{y}}}_k}-{\mathit{K}}_k{\mathit{B}}_{0,k}\right)\end{array} \end{gathered}$$

(39)

where the system and measurement noise covariances are Gaussian white noises, i.e., $\left({\mathit{r}}_i{\mathit{r}}_j{}^T\right)=0,$ $E\left({\mathit{q}}_i{\mathit{q}}_j{}^T\right)=0$ and $E\left({\mathit{q}}_i{\mathit{r}}_j{}^T\right)=0\left(i\ne j\right)$, and ${\mathit{B}}_{0,k}$ is the covariance of innovation vector ${\mathit{Z}}_k^I$.

To satisfy the condition (28), (39) is required to be zero, leading to:

$${\overline{\mathit{P}}}_{{\overline{\mathit{x}}}_k{\overline{\mathit{y}}}_k}-{\mathit{K}}_k{\mathit{B}}_{0,k}=0.$$

(40)

By Taylor series, the predicted measurement covariance given by (11) can be further written as:

$$\begin{array}{l}{\mathit{P}}_{{\overline{\mathit{y}}}_k}={\displaystyle {\displaystyle \sum }_{i=1}^{2N}}{w}_i^c\left({\overline{\mathit{y}}}_k^{\left(i\right)}-{\overline{\mathit{y}}}_k\right){\left({\overline{\mathit{y}}}_k^{\left(i\right)}-{\overline{\mathit{y}}}_k\right)}^T+{\mathit{R}}_k\\ ={\mathit{H}}_k{\overline{\mathit{P}}}_k{\mathit{H}}_k{}^T+{\mathit{R}}_k.\end{array}$$

(41)

Replacing ${\overline{\mathit{P}}}_k$ with the corrected state covariance ${\overline{\mathit{P}}}_k^{*}$ in (26) yields:

$$\begin{array}{l}{\mathit{P}}_{{\overline{\mathit{y}}}_k}^{*}=\left[{\mathit{H}}_k{\mathit{\gamma }}_k{\overline{\mathit{P}}}_k{\mathit{H}}_k^T\right]+{\mathit{R}}_k\\ ={\gamma }_k\left[{\mathit{H}}_k{\overline{\mathit{P}}}_k{\mathit{H}}_k{}^T\right]+{\mathit{R}}_k\end{array}$$

(42)

where ${\mathit{P}}_{{\overline{\mathit{y}}}_k}^{*}$ denotes the corrected predicted measurement covariance.

Replacing ${\overline{\mathit{P}}}_k$ with the corrected state covariance ${\overline{\mathit{P}}}_k^{*}$ in (37) yields:

$${\mathit{P}}_{{\overline{\mathit{x}}}_k{\overline{\mathit{y}}}_k}={\gamma }_k{\overline{\mathit{P}}}_k{\mathit{H}}_k{}^T.$$

(43)

Substituting (42) and (43) into (40), we have:

$${\gamma }_k\left[{\mathit{H}}_k{\overline{\mathit{P}}}_k{\mathit{H}}_k{}^T\right]={\mathit{B}}_{0,k}-{\mathit{R}}_k.$$

(44)

To determine ${\gamma }_k$, we need to know innovation covariance ${\mathit{B}}_{0,k}$ of innovation vector ${\mathit{Z}}_k^I$, which is defined by:

$${\mathit{B}}_{0,k}=E\left({\mathit{Z}}_k^I{\mathit{Z}}_k^I{}^T\right).$$

(45)

Consider a time window of width M, i.e., there are M time points $t_{k-1},t_{k-2},\dots ,t_{k-M}$ in the time window. Thus, (45) can be further written as:

$${\mathit{B}}_{0,k}=\frac{1}{M}{\displaystyle {\displaystyle \sum }_{j=1}^M}{\mathit{Z}}_{k-j}^I{\mathit{Z}}_{k-j}^I{}^T.$$

(46)

Applying the random weighting concept [21] to (46), the random weighting estimation of ${\mathit{B}}_{0,k}$ can be obtained as:

$${\mathit{B}}_{0,k}={\displaystyle {\displaystyle \sum }_{j=1}^M}{v}_j{\mathit{Z}}_{k-j}^I{\mathit{Z}}_{k-j}^I{}^T.$$

(47)

where $v_j$ is the random weighting factor which meets the condition ${{\displaystyle \sum }}_{j=1}^M{v}_j=1$.

Substituting (47) into (44) yields:

$${\gamma }_k\left[{\mathit{H}}_k{\overline{\mathit{P}}}_k{\mathit{H}}_k{}^T\right]={\displaystyle \sum }_{j=1}^M{v}_j{\mathit{Z}}_{k-j}^I{\mathit{Z}}_{k-j}^I{}^T-{\mathit{R}}_k$$

(48)

Thus, ${\gamma }_k$ is determined as:

$${\gamma }_k=\frac{tr\left({{\displaystyle \sum }}_{j=1}^M{v}_j{\mathit{Z}}_{k-j}^I{\mathit{Z}}_{k-j}^I{}^T\right)-tr\left({\mathit{R}}_k\right)}{tr\left({\mathit{H}}_k{\overline{\mathit{P}}}_k{\mathit{H}}_k{}^T\right)}.$$

(49)

The proof of Theorem 1 is completed. □

According to Theorem 1, the corrected predicted state covariance can be calculated as:

$${\overline{\mathit{P}}}_k^{*}=\left(\frac{t\mathit{r}\left({{\displaystyle \sum }}_{j=1}^M{v}_j{\mathit{Z}}_{k-j}^I{\mathit{Z}}_{k-j}^I{}^T\right)-tr\left({\mathit{R}}_k\right)}{tr\left({\mathit{H}}_k{\overline{\mathit{P}}}_k{\mathit{H}}_k{}^T\right)}\right){\overline{\mathit{P}}}_k.$$

(50)

From the above, we can see that the proposed RWSTUKF takes into account model error by correcting the predicted state covariance. Further, the determination process of the scaling factor ${\gamma }_k$ does not involve the calculation of Jacobian matrix.

4.2. Identification of Model Error

Mahalanobis distance is a popular means to detect an uncertain condition [27,28,29]. In this paper, the concept of Mahalanobis distance is adopted to identify contact model error. The Mahalanobis distance ${\theta }_k$ is defined by the innovation and predicted measurement covariance as:

$${\theta }_k={\mathit{Z}}_k^I{}^T{\overline{\mathit{P}}}_{{\overline{\mathit{y}}}_k}^{-1}{\mathit{Z}}_k^I.$$

(51)

${\theta }_k$ is calculated during the standard UKF procedure to identify contact model error. A contact model error is identified according to the following conditions:

$$\{\begin{array}{l}if{\theta }_k\le {\theta }_{T}Inexistenceofmodelerror\hfill \\ if{\theta }_k>{\theta }_{T}Existenceofmodelerror\hfill \end{array}$$

(52)

where ${\theta }_T$ denotes the predefined threshold value.

4.3. Algorithm

The detailed procedure of the proposed method is illustrated in Figure 1. In the absence of model error, the proposed method just follows the standard UKF procedure. In the presence of model error, the predicted state covariance is corrected to compensate contact model error and further re-estimate the system state. It can be seen from Figure 1, the proposed method only repeats the process of measurement update (i.e., Step 3), thus maintaining the computational efficiency.

5. Performance Evaluation and Discussions

Simulations and experiments were conducted to comprehensively evaluate the proposed method (i.e., RWSTUKF). Simulation analysis was conducted to evaluate the performance of RWSTUKF under three different kinds of contact model error, i.e., initial state estimation error, model simplification error, and local modelling error. Monte Carlo simulations were carried out 100 times. A master-slave robotic indentation system was also developed to conduct experiments for the performance evaluation. In both simulation and experimental analyses, the input force signals are generated based on the nonlinear H-C contact model. The contact forces are reconstructed from the estimated parameters of the nonlinear H-C model and further compared with the input forces as reference to calculate the estimation error. Comparison analysis of the proposed RWSTUKF with the conventional UKF [17] is also discussed in this section.

5.1. Initial State Estimation Error

Consider the nonlinear H-C contact model described by (1) with the following constant parameters:

$$K_k=10,B_k=1,n_k=2,p_k=1.05$$

(53)

by which the input forces are generated from continuously increased displacements. In order to simulate the initial state error, the initial values of the H-C model parameters are set to:

$$K_0=150,B_0=2,n_0=1,p_0=1.$$

(54)

By comparing (54) with (53), it is obvious that the initial value of state estimation involves a large error.

Trials were conducted by both UKF and RWSTUKF under the same conditions to analyse the effect of the initial state estimation error. The displacement velocity was set to $\dot{d}(t_k)=$ 0.1 mm, and the window size m = 4. ${\mathit{Q}}_k$ was set to $diag{\left(0.01mN\right)}_{7\times 7}$, and ${\mathit{R}}_k$ $diag{\left(0.01mN\right)}_{2\times 2}$.

Figure 2 shows the estimation errors by both UKF and RWSTUKF under the initial state estimation error. Due to the influence of the initial error, the estimation error by UKF is rapidly increased and then converged after 150 time steps, leading to the maximum estimation error of 74.265 mN. However, the estimation error by RWSTUKF is converged only after 50 time steps, showing that the convergence speed of RWSTUKF is three times faster than that of UKF. The resultant maximum estimation error by RWSTUKF is 13.887 mN, which is about six times smaller than that by UKF. This is because RWSTUKF can dynamically adjust predicted state covariance to restrain the disturbance of the initial error on the filtering solution, leading to the improved estimation accuracy than UKF.

Table 1. Estimation errors by both UKF and RWSTUKF under the initial state estimation error.

Errors (mN)	UKF	RWSTUKF
Mean error	16.8818	1.8092
Max error	74.2650	13.8870
RMSE	30.2395	2.9133

summarizes the estimation errors by both UKF and RWSTUKF. The mean error and RMSE (root mean square error) are 1.8092 mN and 2.9133 mN for RWSTUKF, whereas they are 2.9133 mN and 30.2395 mN for UKF.

5.2. Model Simplification Error

Consider the error of model simplification. By letting the parameter p = 1, the nonlinear H-C contact model given by (1) is simplified as:

$$F=K{d}^n+B{d}^n\dot{d}$$

(55)

The input forces are still generated according to (1) under the same conditions as the simulation case in Section 5.1 except that the initial parameter values are set to:

$$K_0=10,B_0=1,n_0=2,p_0=1.05$$

(56)

Trials were conducted by both UKF and RWSTUKF under the same conditions to analyse the effect of the model simplification error. The displacement velocity was $\dot{d}{\left(t\right)}_k$ = 0.01, and the window size m = 4. ${\mathit{Q}}_k$ was set to $diag{\left(0.1mN\right)}_{7\times 7}$ and ${\mathit{R}}_k$ to $diag{\left(0.1mN\right)}_{2\times 2}.$

Figure 3 shows the estimation errors by both UKF and RWSTUKF under the error of model simplification. The estimation error of UKF is bounded by the maximum of 0.4582 mN within 200 time steps. However, after 200 time steps, due to the disturbance by the error of model simplification, the estimation error of UKF is drastically increased, leading to the maximum error of 1.4844 mN at the end of the test time period. In contrast, within 40 time steps, the estimation error of RWSTUKF is very small, leading to the maximum of 0.0444 mN. After 40 time steps, in spite of a relatively large increase, the estimation error of RWSTUKF is bounded by the maximum of 0.3039 mN, which is still smaller than the maximum error of UKF within 200 time steps. This demonstrates RWSTUKF can restrain the disturbance due to the error of model simplification.

Table 2. Estimation errors by both UKF and RWSTUKF under the error of model simplification.

Errors (mN)	UKF	RWSTUKF
Mean error	0.4068	0.0897
Max error	1.4844	0.3039
RMSE	0.5394	0.1063

shows the estimation errors by both UKF and RWSTUKF. The mean error and RMSE are 0.4068 mN and 0.5394 mN for UKF, while they are 0.08977 mN and 0.1063 mN for RWSTUKF. Thus, it is clear that RWSTUKF outperforms UKF.

5.3. Local Modelling Error

To simulate local modelling error, a constant prediction error of [0 0 0 0.8 0.8 0 0] is added to the predicted state described by (8) for the time period (200~220 time steps). The other parameters are the same as the simulation case in Section 5.1, except that the initial parameter values are set to be the same as the true values.

Figure 4 shows the estimation errors by both UKF and RWSTUKF in the presence of the local modelling error. It is clear that the estimation error of UKF is increased dramatically during the time period (200~220 time steps) with the added constant error, leading to the maximum estimation error of 6.7853 mN. In contrast, the estimation error curve of RWSTUKF does not involve a significant change during the entire test time period, especially for the time period with the added constant error. This demonstrates that RWSTUKF can handle the local modelling error. As shown in

Table 3. Estimation errors by both UKF and RWSTUKF under local modelling error.

Errors (mN)	UKF	RWSTUKF
Mean error	0.9531	0.6911
Max error	6.7853	2.5880
RMSE	1.4200	0.8590

, the maximum estimation error is 6.7853 mN for UKF, while it is only 2.588 mN for RWSTUKF, which is about three times smaller than that of UKF. The mean error and RMSE are 0.9531 mN and 1.42 mN for UKF, while they are 0.6911 mN and 0.8590 mN for RWSTUKF.

5.4. Robotic Indentation

A master-slave robotic indentation system was developed for the purpose of experimental verification. As shown in Figure 5, the master robot is a Phantom Omni haptic device. The slave robot consists of a linear magnetic motor (LM2070_08011_FMM, FAULHABER) and an indenter of diameter 5 mm, together with a six-axis force sensor (Nano 17 and FTIFPS1) attached between the linear motor and indenter to measure the contact force with the phantom tissue sample. The slave robot can be moved back and forth by manipulating the master robot. With the master-slave robotic system, users can conduct mechanical indentation test on soft material samples to record the contact force and displacement.

A phantom cubic-shape (3 cm × 8 cm × 6 cm) soft tissue was produced from silicone rubber (Ecoflex 0030), which has similar characteristics with human tissues [30]. For the comparison analysis purpose, trials were conducted by both conventional UKF and proposed RWSTUKF under the same conditions. The size of window m was 5, ${\mathit{x}}_0=\left[0.1,0.1,0.01,0.0001,0.0001,11\right]$, ${\mathit{Q}}_k$ was set to $diag{\left(0.01mN\right)}_{7\times 7}$, and ${\mathit{R}}_k$ was set to $diag{\left(0.1mN\right)}_{2\times 2}.$

Figure 6 shows the reconstructed forces by both UKF and RWSTUKF with reference to the measured contact force. It can be seen that the UKF estimation involves a large error, leading to the maximum error of 9.6501 N. In contrast, the RWSTUKF estimation follows the reference force curve more closely, leading to the maximum error of 3.3760 N, which is almost three times smaller than that of UKF. This is because RWSTUKF has the capability to handle system model error by online correction of predicted state covariance.

Table 4. Estimation errors of both UKF and RWSTUKF for robotic indentation.

Errors (N)	UKF	RWSTUKF
Mean error	0.4131	0.2624
Max error	9.6501	3.3760
RMSE	0.9332	0.5088

lists the estimation errors of UKF and RWSTUKF. It can be seen that in addition to the maximum error, the mean error and RMSE of RWSTUKF are also much smaller than those of UKF.

6. Conclusions

This paper presents a new method based on the nonlinear H-C contact model for nonlinear soft tissue characterization in the presence of model error. This method identifies model error using the Mahalanobis distance and further incorporates a dynamic scaling factor in predicted state covariance to online compensate identified model error. This scaling factor is determined by combining the principle of innovation orthogonality with the random weighting concept to avoid the cumbersome computation of Jacobian matrix and provide reliable estimation for innovation covariance. The proposed method not only outperforms UKF in the presence of model error, but it also maintains the computational efficiency by correcting predicted state covariance only in the time segments with contact model error. Simulation and experimental results as well as comparison analysis demonstrate that the proposed method can effectively restrain the disturbance of contact model error for online soft tissue characterization.

Future work will focus on the improvement of the proposed method for online soft tissue characterization. It is expected to combine the proposed with artificial intelligence techniques such as genetic algorithms, neural network, pattern recognition and machine learning to automatically determine optimal random weights for the covariance of innovation vector according to the disturbance of abnormal measurements, thus automatically handling contact model error from various sources.