Discrete Unilateral Constrained Extended Kalman Filter in an Embedded System

Abstract

Since its publication in the 1960s, the Kalman Filter (KF) has been a powerful tool in optimal state estimation. However, the KF and most of its variants have mainly focused on the state estimation of smooth systems. In this work, we propose a new algorithm called the Discrete Unilateral Constrained Extended Kalman Filter (DUCEKF) that expands the capabilities of the Extended Kalman Filter (EKF) to a class of hybrid mechanical systems known as systems with unilateral constraints. Such systems are non-smooth in position and discontinuous in velocity. Lyapunov stability theory is invoked to establish sufficient conditions for the estimation error stability of the proposed algorithm. A comparison of the proposed algorithm with the EKF is conducted in simulation through a case study to demonstrate the superiority of the DUCEKF for the state estimation tasks in this class of systems. Simulations and an experiment were developed in this case study to validate the performance of the proposed algorithm. The experiment was conducted using electronic hardware that consists of an Embedded System (ES) called “Mikromedia for dsPIC33EP” and an external DAC-12 Click board, which includes a Digital-to-Analog Converter (DAC) from Texas Instruments.

1. Introduction

The Kalman Filter (KF) [1], proposed in the 1960s, is an optimal state estimator that has been used in many applications. Over the years, its capabilities have been expanded to generalize to a wide range of systems and scenarios. Although most of these generalizations are focused on smooth systems, there have been a few works on non-smooth systems, e.g., [2,3,4,5,6,7,8].

This work aims to expand the capabilities of the EKF to a class of hybrid mechanical systems known as systems with unilateral constraints. In such a system, the unilateral constraint restricts the system’s motion in only one direction, which prevents penetration between the system and an impact surface while still allowing for impacts to occur. As an example, a ball resting on a table is a system under unilateral constraints because it cannot move downward through the table but is free to move in other directions. These systems are characterized by differential and difference equations with a switching rule that defines the commutation between these equations. Such systems are non-smooth in position and discontinuous in velocity at the impact surface, where the discrete dynamic comes into play. These systems have received much attention, especially in control engineering [9,10,11,12,13]. Examples of them include the walking biped robot [14], Newton’s pendulum [15], the pendulum with barrier [12], and the Bouncing Ball [16].

This paper builds upon [17] to meet real-world applications typically conceived in digital implementations. In this paper, the differential equation of the system is discretized for digital implementation. As a result, two different equations and a switching rule define the system with unilateral constraints treated in this research. The difference equation derived from the differential equation describes the dynamics beyond the impact surface, while the other equation defines the dynamics at this surface. The resulting system remains non-smooth in position and discontinuous in velocity in the presence of impacts.

The DUCEKF is introduced for the state estimation in such a system. The DUCEKF consists of the discrete EKF, a discrete transition, and a switching rule. The EKF performs estimations beyond impacts in the smooth branch. On the other side, the discrete transition is responsible for estimations at the impact surface, where the non-smooth branch occurs. Sufficient conditions for the stability of the proposed algorithm are derived based on Lyapunov stability theory. Since the DUCEKF is defined for discrete systems, the employed Lyapunov theory is discrete. This differs from the continuous treatment used in [17] and ensures stability within this discrete framework, which is essential for embedded implementation.

A Lyapunov function depending on the DUCEKF Difference Riccati Equation (DRE) solution and estimation error is defined for the non-smooth branch. The function guides the online modification of the DRE solution, which ensures that the Lyapunov function does not grow after impacts. The modified DRE solution and the estimations at the non-smooth branch constitute the DUCEKF at this phase and are employed to reinitialize the EKF to take over again at the smooth branch. Stability is a significant contribution, as in previous studies on non-smooth KF-based estimations, the stability of the estimation error is not often focused [2,3,4,6,8]. However, some works, such as [5,7], do emphasize Lyapunov stability and robustness.

In Matlab, the DUCEKF is numerically exemplified with a case study, the Discrete Hybrid Van der Pol Oscillator (DHVPO), a discrete version of the Hybrid Van der Pol Oscillator from [18]. The DUCEKF is also compared with the EKF in the case study to show the advantages of the DUCEKF over the EKF in the state estimation task of systems with unilateral constraints. The superiority of DUCEKF over EKF is evident by looking at the behavior of the Lyapunov function, which shows stability for the DUCEKF estimations while showing instability for the EKF estimations.

After simulations in MATLAB 2024b, the algorithm is implemented in the ES “Mikromedia for dsPIC33EP” to meet real-world applications. The ES is powered by a 16-bit dsPIC33EP512MU810 microcontroller and features the Thin Film Transistor (TFT) Liquid Crystal Display (LCD) module for multimedia applications, where the graphical results on the real and estimated phase portraits are displayed for comparison.

As the algorithm was initially developed in MATLAB, the algorithm was ported to the C programming language using the MikroC PRO for dsPIC. The code was developed for implementation on the Mikromedia dsPIC33EP board. This compact and efficient development platform enables real-time user interaction and visual monitoring of system behavior [19].

An external DAC-12 Click board was integrated via SPI with the ES to enhance the system’s functionality. This board includes the DAC60508, an eight-channel DAC from Texas Instruments, which generates analog output signals corresponding to the estimated and real states. These signals are displayed on an oscilloscope as a time series for comparison of real vs. estimated. C code was also developed in the ES for DAC integration.

The ES implementation proves that the proposed DUCEKF can be implemented in an ES, as the EKF [20,21]. ES implementation is a significant contribution, as embedded systems are the means to real-world applications such as the automotive industry, medical services, the Internet of Things, security systems, etc.

The proposed algorithm is well-suited for sensor data fusion and state estimation in systems operating under unilateral constraints. It advances standard sensor data fusion techniques, primarily based on KF and its variants, which are designed for smooth systems. Additionally, this algorithm can function as a state estimator for unmeasured states, thereby reducing the cost or complexity associated with sensors.

The organization of the document is as follows: the preliminaries are in Section 2, the DUCEKF in Section 3, the electronic implementation in Section 4, the conclusions in Section 5, and an appendix detailing the algorithm implementation is in Appendix A.

2. Preliminaries

For later use, the discrete EKF, the discrete system with unilateral constraints, and the description of the DHVPO employed to exemplify the proposed approach are detailed.

2.1. Discrete Extended Kalman Filter

Consider the nonlinear systems described by the difference equation

$$x_k=f\left(x_{k-1}\right)+w_k,$$

(1)

where $x_{k-1}={\left[x_{k-1}{\left(1\right)}^{\top }{x}_{k-1}{\left(2\right)}^{\top }\right]}^{\top }$ is the state vector, $f\left(x_{k-1}\right)={\left[f_1{\left(x_{k-1}\right)}^{\top }{f}_2{\left(x_{k-1}\right)}^{\top }\right]}^{\top }$ a nonlinear function that defines the dynamics of the system, and $w_k$ is a vector of White Gaussian Noise (WGN) normally distributed as $\mathcal{N}(0,Q_k)$, with 0 mean and covariance matrix $Q_k$. For (1), the output is

$$y_k=h\left(x_k\right)+v_k,$$

(2)

where $h\left(x_k\right)$ is a vector function of the state vector, and $v_k$ is the vector of WGN normally distributed as $\mathcal{N}(0,R_k)$, where 0 is the mean and $R_k$ is the covariance matrix of $v_k$.

The EKF estimates the state vector of (1) from the combination of the output (2) and the model of (1).

The EKF can be found in the literature [22], and its description is as follows:

$$\begin{array}{ccc}\hfill {\widehat{x}}_k^p& =& f\left({\widehat{x}}_{k-1}^u\right),\hfill \end{array}$$

(3)

$$\begin{array}{ccc}\hfill P_k^p& =& A_{k-1}{P}_{k-1}^u{A}_{k-1}^{\top }+Q_k,\hfill \end{array}$$

(4)

$$\begin{array}{ccc}\hfill K_k& =& P_k^p{C}_k^{\top }{(C_k{P}_k^p{C}_k^{\top }+R_k)}^{-1},\hfill \end{array}$$

(5)

$$\begin{array}{ccc}\hfill {\widehat{x}}_k^u& =& {\widehat{x}}_k^p+K_k(y_k-h\left({\widehat{x}}_k^p\right)),\hfill \end{array}$$

(6)

$$\begin{array}{ccc}\hfill P_k^u& =& (I-K_k{C}_k)P_k^p.\hfill \end{array}$$

(7)

Description of the variables in the EKF are given below:

• ${\widehat{x}}_k^p={\left[{\widehat{x}}_k^p{\left(1\right)}^{\top }{\widehat{x}}_k^p{\left(2\right)}^{\top }\right]}^{\top }$ is the estimation of the state $x_k$ before $y_k$.
• $P_k^p$ is the predicted covariance matrix of the estimation error.
• $K_k$ is the Kalman gain.
• ${\widehat{x}}_k^u={\left[{\widehat{x}}_k^u{\left(1\right)}^{\top }{\widehat{x}}_k^u{\left(2\right)}^{\top }\right]}^{\top }$ describes, although locally, the optimal estimation of $x_k$.
• $P_k^u$ is the updated covariance matrix of the estimation error.
• $Q_k$ and $R_k$ are symmetric and positive definite matrices capturing in their diagonals the variances of $w_k$ and $v_k$, respectively.

The superindexes p and u indicate the prediction and update phases of the EKF. The matrices

$$\begin{array}{c}\hfill A_{k-1}={\displaystyle \frac{\partial f\left(x_{k-1}\right)}{\partial x_{k-1}}}{|}_{{\widehat{x}}_{k-1}^u},C_k={\displaystyle \frac{\partial h\left(x_k\right)}{\partial x_k}}{|}_{{\widehat{x}}_k^p}\end{array}$$

result from the linear approximations of (1) and (2) around the nominal values $x_{k-1}={\widehat{x}}_{k-1}^u$, $x_k={\widehat{x}}_k^p$, $w_k=0$, and $v_k=0$. These approximations are

$$\begin{array}{ccc}\hfill x_k& \approx & A_{k-1}{x}_{k-1}+u_{k-1}+w_k\mathrm{and}\hfill \end{array}$$

(8)

$$\begin{array}{ccc}\hfill y_k& \approx & h\left({\widehat{x}}_k^p\right)+C_k{x}_k-C_k{\widehat{x}}_k^p+v_k,\hfill \end{array}$$

(9)

with $u_{k-1}=f\left({\widehat{x}}_{k-1}^u\right)-A_{k-1}{\widehat{x}}_{k-1}^u$. The idealized initial conditions for the EKF are ${\widehat{x}}_0^u=E\left[x_0\right]$ and $P_0^u=$ var$\left[x_0\right]$, where E and var are, respectively, the expectation and variance operators.

2.2. Discrete System with Unilateral Constraints

Given the unilateral constraint $F\left(x_k\left(1\right)\right)\ge \epsilon$, the discrete system with unilateral constraints is described by (10).

$$\mathrm{System}=\left\{\begin{array}{cc}\mathrm{Smooth}\mathrm{branch}\left(1\right)–\left(2\right),\hfill & F(x_k\left(1\right)\mathrm{})>\epsilon \hfill \\ \mathrm{Non}–\mathrm{smooth}\mathrm{branch}\left(11\right),\hfill & F\left(x_{i-1}\left(1\right)\right)=\epsilon \hfill \end{array}\right.$$

(10)

Equations (1) and (2) hold beyond the impact surface $F\left(x_{i-1}\left(1\right)\right)=\epsilon$, and (11) takes over at this surface when an impact occurs at the discrete time $i-1$. The function $\mu \left(x_{i-1}\right)={\left[x_{i-1}{\left(1\right)}^{\top }{\mu }_1{\left(x_{i-1}\right)}^{\top }\right]}^{\top }$ defines a constant position and discontinuous velocity in this surface, $x_i={\left[x_i{\left(1\right)}^{\top }{x}_i{\left(2\right)}^{\top }\right]}^{\top }$ is the state vector at this point, and $\epsilon =[{\epsilon }_0,0)$ is an activation set that defines a range for all the $x_{i-1}$ events, with ${\epsilon }_0>0$.

$$x_i=\mu \left(x_{i-1}\right).$$

(11)

2.3. Discrete Hybrid Van der Pol Oscillator

A case study system that will be used later to exemplify our contribution is the DHVPO, a discrete version of the oscillator from [18]. Such an oscillator can produce a discontinuous limit cycle that bifurcates into an asymptotically stable origin under certain parameters. Its solution has served as a reference in the control of mechanical systems [12,23]. Its discrete version forms the case study system that follows the generic definition (10). Given the unilateral constraint $\underset{F\left(x_k\left(1\right)\right)}{\underbrace{x_k\left(1\right)}}\ge \epsilon$, the oscillator is described by (12).

$$\mathrm{DHVPO}=\left\{\begin{array}{cc}\mathrm{Continuous}\mathrm{Van}\mathrm{der}\mathrm{Pol}\left(13\right)–\left(14\right),\hfill & x_k\left(1\right)>\epsilon \hfill \\ \mathrm{Transition}\left(15\right),\hfill & x_{i-1}\left(1\right)=\epsilon ,x_k\left(2\right)<0\hfill \end{array}\right.$$

(12)

Beyond the impact surface, when $x_k\left(1\right)>\epsilon$, the oscillator is governed by (13) and (14). On the other side, it is described by (15) when the impact surface is reached at $x_{i-1}\left(1\right)=\epsilon$ and $x_k\left(2\right)<0$. In addition to the position constraint $x_{i-1}\left(1\right)=\epsilon$, the velocity constraint $x_k\left(2\right)<0$ is considered to activate the non-smooth phase in this case study.

$$\underset{x_k}{\underbrace{\left[\begin{array}{c}{x}_k\left(1\right)\\ x_k\left(2\right)\end{array}\right]}}=\underset{f\left(x_{k-1}\right)}{\underbrace{\left[\begin{array}{c}{x}_{k-1}\left(1\right)\\ x_{k-1}\left(2\right)\end{array}\right]+\left[\begin{array}{c}{x}_{k-1}\left(2\right)\\ -\alpha \left[\left(x_{k-1}{\left(1\right)}^2+{\displaystyle \frac{x_{k-1}{\left(2\right)}^2}{{\mu }^2}}\right)-{\rho }^2\right]x_{k-1}\left(2\right)-{\mu }^2{x}_{k-1}\left(1\right)\end{array}\right]\Delta T}}+\underset{w_k}{\underbrace{\left[\begin{array}{c}{w}_k\left(1\right)\\ w_k\left(2\right)\end{array}\right]}}$$

(13)

$$y_k=\underset{h\left(x_k\right)}{\underbrace{x_k\left(1\right)}}+v_k$$

(14)

$$\underset{x_i}{\underbrace{\left[\begin{array}{c}{x}_i\left(1\right)\\ x_i\left(2\right)\end{array}\right]}}=\underset{\mu \left(x_{i-1}\right)}{\underbrace{\left[\begin{array}{c}{x}_{i-1}\left(1\right)\\ -\kappa x_{i-1}\left(2\right)\end{array}\right]}}$$

(15)

In the transition (15), $i=1,2,\cdots$ are impact instants that occur when the oscillator hits the impact surface. According to [18], a discontinuous limit cycle is generated when the bifurcation parameters $\alpha =\mu =1, \rho =1.5,$ and $\kappa =0.5$ are defined. The Euler method was employed to get the discrete version (13). Assuming $x_k\left(1\right)$ and $x_k\left(2\right)$ as the position and velocity of a mechanical system, only position measurements are considered to be available, i.e., $h\left(x_k\right)=x_k\left(1\right)$ in (2).

3. Discrete Unilateral Constrained Extended Kalman Filter

Given the unilateral constraint $F\left({\widehat{x}}_k^u\left(1\right)\right)\ge \epsilon$, the proposed filter is defined by (16). Such a filter is constituted by the discrete EKF (3)–(7), the transition (17)–(18), and the switching rule defined within (16).

$$\mathrm{DUCEKF}=\left\{\begin{array}{cc}\mathrm{EKF}\left(3\right)–\left(7\right),\hfill & F\left({\widehat{x}}_k^u\left(1\right)\right)>\epsilon \hfill \\ \mathrm{Transition}\left(17\right)–\left(18\right),\hfill & F\left({\widehat{x}}_{i-1}^u\left(1\right)\right)=\epsilon \hfill \end{array}\right.$$

(16)

Beyond the impact surface, when $F\left({\widehat{x}}_k^u\left(1\right)\right)>\epsilon$, the DUCEFK is the discrete EKF. However, when the surface is reached at $F\left({\widehat{x}}_{i-1}^u\left(1\right)\right)=\epsilon$, the DUCEFK is defined by the transition (17)–(18).

$${\widehat{x}}_i^u=\mu \left({\widehat{x}}_{i-1}^u\right)$$

(17)

$$P_i^u=\mathrm{\Omega }{P}_{i-1}^u$$

(18)

Equation (17) defines a constant estimated position and discontinuous estimated velocity, where ${\widehat{x}}_i^u={\left[{\widehat{x}}_i^u{\left(1\right)}^{\top }{\widehat{x}}_i^u{\left(2\right)}^{\top }\right]}^{\top }$ and $\mu \left({\widehat{x}}_{i-1}^u\right)={\left[{\widehat{x}}_{i-1}^u{\left(1\right)}^{\top }{\mu }_1{\left({\widehat{x}}_{i-1}^u\right)}^{\top }\right]}^{\top }$. Equation (18) defines the propagation of the updated covariance matrix, and sufficient conditions are derived in the next theorem, right after the assumption, for this to guarantee asymptotic stability in the Lyapunov sense.

Assumption 1.

Assume that the system and the DUCEKF trajectories reach the impact surfaces $F\left(x_{i-1}\left(1\right)\right)=\epsilon$ and $F\left({\widehat{x}}_{i-1}^u\left(1\right)\right)=\epsilon$ simultaneously, then the estimation error at this surface is

$$e_i=\mu (e_{i-1}+{\widehat{x}}_{i-1}^u))-\mu \left({\widehat{x}}_{i-1}^u\right),$$

(19)

where $e_i={\left[e_i{\left(1\right)}^{\top }{e}_i{\left(2\right)}^{\top }\right]}^{\top }={[x_i{\left(1\right)}^{\top }-{\widehat{x}}_i^u{\left(1\right)}^{\top }{x}_i{\left(2\right)}^{\top }-{\widehat{x}}_i^u{\left(2\right)}^{\top }]}^{\top }$.

Theorem 1.

Provided that the previous assumption is satisfied, the propagation of the covariance defined by (18) establishes sufficient conditions for the Lyapunov stability of the estimation error (19).

Proof. 

Considere the Lyapunov inequality

$$V\left(e_i\right)-V\left(e_{i-1}\right)\le 0$$

(20)

expressed as a function of the DRE solution and the estimation error

$$\begin{array}{c}\hfill V\left(\delta e_i\right)-V\left(\delta e_{i-1}\right)=\delta e_i^{\top }{P}_i^u\delta e_i\\ \hfill -\delta e_{i-1}^{\top }{P}_{i-1}^u\delta e_{i-1}\le 0.\end{array}$$

(21)

In (21), $\delta e_i$ is the local post-impact estimation error, coming from the following linearization

$$\delta e_i=\tilde{A}\delta e_{i-1}$$

(22)

of (19) around the nominal value $e_{i-1}=0$, where

$$\tilde{A}={\displaystyle \frac{\partial \left[\mu (e_{i-1}+{\widehat{x}}_{i-1}^u)\right)-\mu \left({\widehat{x}}_{i-1}^u\right)]}{\partial e_{i-1}}}{|}_{e_{i-1}=0}.$$

(23)

Finally (21) can be written as

$$\begin{array}{c}\hfill V\left(\delta e_i\right)-V\left(\delta e_{i-1}\right)=\\ \hfill \delta e_{i-1}^{\top }[{\tilde{A}}^{\top }{P}_i^u\tilde{A}-P_{i-1}^u]\delta e_{i-1}\le 0,\end{array}$$

(24)

where the definition (18), with an appropriate selection of the gain $\mathrm{\Omega }$ forces the inequality (24) to be satisfied. Therefore Lyapunov stability is proved. □

The covariance propagation (18) with an appropriate selection of $\mathrm{\Omega }$ will compensate for any unstable effects produced by the state transition (17). The updated estimate ${\widehat{x}}_i^u$ and covariance $P_i^u$ from (17) and (18) are then utilized to reinitialize the EKF to take over again beyond the impact surface, in the smooth branch. According to the theorem, the gain $\mathrm{\Omega }$ is derived from a systematic approach based on Lyapunov stability analysis. The gain is a key component of the DUCEKF, which is used for state estimation in the generic class of systems defined by Equation (10). The propagation of the covariance matrix is based on the gain to fulfill Lyapunov stability. An explicit solution for this gain can be obtained from Equation (24). In the following section, a case study comparison will demonstrate the significance of this gain; stability will be observed when it is applied, while instability will be evident when it is not.

In addition to the estimation error (19) defined under the assumption, two more definitions of estimation error are feasible. These are shown below.

• The system trajectory reaches the impact surface during the time instants $i=1,2,\cdots$ and the estimated trajectory remains beyond this surface, i.e., $F(e_{i-1}\left(1\right)+{\widehat{x}}_{i-1}^u\left(1\right))=\epsilon$ and $F\left({\widehat{x}}_k^u\left(1\right)\right)\ne \epsilon$. For this scenario, the error is

  $$e_i=\mu (e_{i-1}+{\widehat{x}}_{i-1}^u)-{\widehat{x}}_k^u$$

  (25)

  where ${\widehat{x}}_k^u$ is the estimated state still beyond the impact surface. The time indices i and k are employed to denote the state at this surface and beyond, respectively. However, they occur at the same time.
• The estimated trajectory reaches the constraint during the impact time instants $i=1,2,\cdots$ and the system trajectory remains beyond this surface, i.e., $F\left({\widehat{x}}_{i-1}^u\left(1\right)\right)=\epsilon$ and $F(e_k\left(1\right)+{\widehat{x}}_k^u\left(1\right))\ne \epsilon$. For this scenario, the error is

  $$e_i=e_k+{\widehat{x}}_k^u-\mu \left({\widehat{x}}_{i-1}^u\right)$$

  (26)

  where $e_k+{\widehat{x}}_k^u$ is the system’s state still beyond the impact surface. Again, the time indexes i and k are employed to denote state at this surface and beyond, respectively. However, they occur at the same time.

To keep the assumption, the DUCEKF is synchronized online by the trajectory of the system, i.e., the unilateral constraint $F\left({\widehat{x}}_i^u\left(1\right)\right)\ge 0$ of the DUCEKF is replaced by that of the system, $F\left(x_i\left(1\right)\right)\ge 0$, to produce a reset event depending on the system. The DUCEKF at the impact surface is then described by (17) and (18), provided that $F\left(x_{i-1}\left(1\right)\right)=\epsilon$ is satisfied.

If the system’s trajectory intersects with the impact surface before the estimated trajectory does, (11) and (17)–(18) are activated for both trajectories, regardless of the estimated trajectory’s location. However, if the estimated trajectory reaches the impact surface first, it will remain there, waiting for the system to make contact. Once the system hits the surface, (11) and (17)–(18) take place.

As per synchronization at the impact surface, the state of the system is assumed to be known at this point. This assumption is valid because a sensor that measures impacts, such as a force sensor, can be integrated into real systems.

4. Results

This section presents the results of the DUCEKF (16) performance for the case study DHVPO (12). Both numerical results obtained from MATLAB simulations and experimental results from the electronic hardware are included. The parameters and initial conditions used for this case study are listed in

Table 1. Parameters and initial conditions.

Parameters	Values	Initial Conditions	Values
$Q_k$	$0.001I_{2\times 2}$	${\widehat{x}}_0^u\left(1\right)$	0
$R_k$	$0.0001$	${\widehat{x}}_0^u\left(2\right)$	$1.8$
$\epsilon$	$0.02$	$x_0\left(1\right)$	0
C	$\left[10\right]$	$x_0\left(2\right)$	$1.5$
A	Equation (27)	-	-
$\mathrm{\Omega }$	Equation (28)	$P_0^u$	$0.2I_{2\times 2}$
$\tilde{A}$	Equation (30)	-	-
$\Delta T$	0.01	-	-
$w_k, v_k$	0	-	-

. The parameter $\mathrm{\Omega }$ is defined as in (28) to fulfill the Lyapunov stability expressed in (24), turning (24) into (29). Equations (27) and (30) provide an explicit definition for the parameters A and $\tilde{A}$.

$$A=\left[\begin{array}{cc}1& \Delta T\\ (-2\alpha {\widehat{x}}_{k-1}^u\left(1\right){\widehat{x}}_{k-1}^u\left(2\right)-{\mu }^2)\Delta T& 1+(-\alpha {\widehat{x}}_{k-1}^u{\left(1\right)}^2-{\displaystyle \frac{3\alpha {\widehat{x}}_{k-1}^u{\left(2\right)}^2}{{\mu }^2}}+\alpha {\rho }^2)\Delta T\end{array}\right].$$

(27)

$$\mathrm{\Omega }=\left[\begin{array}{cc}1& -{\displaystyle \frac{1}{\kappa }}\\ -{\displaystyle \frac{1}{\kappa }}& {\displaystyle \frac{1}{{\kappa }^2}}\end{array}\right].$$

(28)

$$\begin{array}{c}\hfill V\left(\delta e_i\right)-V\left(\delta e_{i-1}\right)=0.\end{array}$$

(29)

$$\tilde{A}=\left[\begin{array}{cc}1& 0\\ 0& -\kappa \end{array}\right].$$

(30)

4.1. MATLAB Simulation

Simulation results from MATLAB are in Figure 1 and Figure 2. Figure 1 shows the time evolution of the estimated and the real states for comparison, whereas Figure 2 shows the phase portraits of the estimated and real states for spatial comparison. Despite different initial conditions, the estimates converge to the real state.

A comparison between the DUCEKF and the EKF is conducted in the numerical setting to demonstrate the advantages of the former over the latter in the case study. To ensure a fair comparison, the constraint (17) is integrated into the EKF at the impact surface. The difference between the two methods lies in the covariance update (18), which is only part of the DUCEKF but not of the EKF. For this comparison, the parameter $\kappa =1.5$ is utilized to create an unstable estimation error transition (19) and to showcase the capabilities of the DUCEKF over the EKF for the unstable scenario.

The advantages of DUCEKF over the EKF are shown by the behavior of the Lyapunov function at the impact surface. For the DUCEKF, the Lyapunov function shows stability by remaining at zero. The function shows stability even for unstable estimation error transitions, thanks to the compensation provided by the covariance update (18). In contrast, the Lyapunov function for the EKF shows instability by remaining positive on the impact surface. The comparison of the Lyapunov functions for DUCEKF and EKF is shown in Figure 3.

4.2. Experimental Results

The hardware setup utilized for implementation is shown in Figure 4. It includes the Mikromedia for dsPIC33EP (U1) and the DAC-12-Click board (U2). U1 features a TFT LCD screen that is used to display phase-plane portraits, whereas U2 features a precise 12-bit DAC60508 from Texas Instruments to output analog states.

In this experiment, the case study DHVPO is not an external hardware system; rather, it consists of state-space discrete equations implemented in the dsPIC within U1. It is implemented in dsPIC along with DUCEKF and additional code subroutines for data transmission to the DAC and TFT-LCD.

The main algorithm, consisting of DHVPO and DUCEKF, was first programmed in MATLAB and then ported to C using MikroC PRO for dsPIC for real-time execution in U1 and U2. Customized functions were developed in C for all matrix-related calculations, such as matrix multiplication, addition, subtraction, and inversion. The functions were aimed to minimize execution time and computational load in addition to their main objective. Communication between U1 and U2 was established using the SPI protocol, with U1 acting as the master and U2 as the slave. The SPI bus control on U1 was configured to operate in 8-bit mode. To complete a data write operation, three 8-bit sequences are required to control U2, resulting in a 24-bit (3-byte) transmission from master U1 to slave U2. The first 8 bits in the transmission define the channel number in the DAC, and the next 16 bits contain the 12 bits of data for the specific state variable, with the remaining 4 bits padded to complete the 24-bit transmission. The transmission was carried out for each of the four channels assigned to the states $x_k\left(1\right),x_k\left(2\right),{\widehat{x}}_k^u\left(1\right),{\widehat{x}}_k^u\left(2\right)$ generated by the DHVPO and DUCEKF, respectively.

The experimental execution time of the algorithm took 68.3 ms per iteration and is detailed below in three processes:

• In the first process, the estimated average execution time of the main algorithm on the dsPIC33EP512MU810 in U1 was 248 µs per iteration. The execution time is based on the clock frequency and code efficiency, which was aimed at minimizing the execution time of operations.
• In the second process, the four state variables $x_k\left(1\right),x_k\left(2\right),{\widehat{x}}_k^u\left(1\right),{\widehat{x}}_k^u\left(2\right)$ were reproduced by DAC60508 in 120 µs per iteration.
• In the third process, a graphic routine generated points and lines to display the phase portraits on the TFT-LCD screen of U1. This process took 67.932 ms per iteration to convert the numerical data into a text string and display it, illustrating estimated and real phase portraits.

The execution time for the three processes is 68.3 ms per iteration. This indicates that the ILI9341 TFT-LCD graphic controller updates a full frame every 68.3 ms under our design conditions, corresponding to a refresh rate of about 14.6 frames per second. The execution time of 68.3 ms is sufficient to reliably display data on the TFT-LCD, ensuring clear and real-time visualization for the end user. A shorter execution time may result in unintelligible data, compromising the clarity of the visual output. The appendix at the end of the document gives a detailed implementation.

Experimental results from hardware are in Figure 5, Figure 6 and Figure 7. Figure 5 shows the time evolution of the position, whereas Figure 6 shows the velocity. The estimated position and velocity exhibit similar amplitude and phase compared with the real ones. The data shown in Figure 5 and Figure 6 are analog outputs produced by the Texas Instruments DAC60508 after being converted from digital to analog. Such data were measured using an oscilloscope and displayed on its screen. Figure 7 shows a comparison of the estimated and real phase portraits displayed on the TFT LCD module of the ES. Tracking of DUCEKF is also confirmed in the phase portraits, despite different initialization.

5. Conclusions

The DUCEKF algorithm was introduced for state estimation in mechanical systems with unilateral constraints. Lyapunov stability theory was utilized to derive sufficient conditions for the algorithm’s estimation error stability. These conditions involved an online modification of the DRE solution in the presence of impacts to ensure Lyapunov stability in this phase. Numerical comparison showed the superiority of the DUCEKF over the EKF in the state estimation task of systems with unilateral constraints. Numerical results also confirmed a successful tracking of the DUCEKF estimated state to the state of the DHVPO, validating the algorithm’s effectiveness and motivating implementation in ES as per its discrete description. Experimental results on the ES integrated with the Texas Instrument DAC also showed a successful tracking of the DUCEKF estimated state to the state of the DHVPO. Experimental performance was shown in both discrete phase portraits displayed in the TFT LCD module and analog time series displayed on the oscilloscope screen. The proposed algorithm and its implementation encourage its application in the automation of various relevant autonomous systems, such as humanoid robots or bipedal walking robots, due to its compact design, affordability, and ability to provide real-time state estimation.