Adaptive Kalman Filtering Developed from Recursive Least Squares Forgetting Algorithms (2024)

Brian Lai and Dennis S. Bernstein Brian Lai and Dennis S. Bernstein are with the Department of Aerospace Engineering, University of Michigan, Ann Arbor, MI, USA. {brianlai, dsbaero}@umich.edu. This work was supported by the NSF Graduate Research Fellowship under Grant No. DGE 1841052.

Abstract

Recursive least squares (RLS) is derived as the recursive minimizer of the least-squares cost function.Moreover, it is well known that RLS is a special case of the Kalman filter.This work presents the Kalman filter least squares (KFLS) cost function, whose recursive minimizer gives the Kalman filter.KFLS is an extension of generalized forgetting recursive least squares (GF-RLS), a general framework which contains various extensions of RLS from the literature as special cases.This then implies that extensions of RLS are also special cases of the Kalman filter.Motivated by this connection, we propose an algorithm that combines extensions of RLS with the Kalman filter, resulting in a new class of adaptive Kalman filters.A numerical example shows that one such adaptive Kalman filter provides improved state estimation for a mass-spring-damper with intermittent, unmodeled collisions.This example suggests that such adaptive Kalman filtering may provide potential benefits for systems with non-classical disturbances.

I Introduction

Despite their respective deterministic and stochastic foundations, least-squares and the Kalman filter share an interconnected history [1].It is well known that the update equations for recursive least squares (RLS) (e.g. [2]) are the same as those of the Kalman filter with, for all $k\geq 0$ , identity state matrix $A_{k}=I$ , zero input matrix $B_{k}=0$ , process noise covariance $\Sigma_{k}=0$ , and measurement noise covariance $\Gamma_{k}=I$ (see p.51 of [3], section 3.3.5 of [4], or p.129 of [5]).As RLS became a foundational algorithm of systems and control theory for online identification of fixed parameters [6, 3], numerous extensions of RLS were developed (e.g. [7, 8, 9, 10, 11, 12, 13, 14, 15, 16]) to improve identification of time-varying parameters.However, little work has been done to connect these extensions to the Kalman filter.

The RLS update equations are often derived as the recursive minimizer of a least squares cost function (e.g. [2]).A natural question is whether the RLS cost function can be generalized to derive the Kalman filter.While other deterministic derivations of the Kalman filter have been presented (e.g. [17, 18]), these do not follow as an extension of the RLS cost.

This work presents the Kalman filter least squares (KFLS) cost function whose recursive minimizer gives the Kalman filter update equations.KFLS is an extension of generalized forgetting recursive least squares (GF-RLS) [19], which contains various extensions of RLS from the literature as special cases.As a result, these extensions of RLS are also special cases of the Kalman filter with particular choices of the process noise covariance matrix.

This result motivates a new class of adaptive Kalman filtering, with modified prior covariance update equations to incorporate forgetting from extensions of RLS.A brief survey is given to show how several forgetting methods from the RLS literature can be applied to adaptive Kalman filtering.A numerical example shows how adaptive Kalman filtering with robust variable forgetting factor [14] improves state estimation of a mass-spring-damper system with intermittent, unmodeled collisions.This example suggests that such an adaptive Kalman filtering may be beneficial when disturbances are non-classical.

I-1 Notation and Terminology

For symmetric $P,Q\in{\mathbb{R}}^{n\times n}$ , $P\prec Q$ (respectively, $P\preceq Q$ ) denotes that $Q-P$ is positive definite (respectively, positive semidefinite).For all $x\in{\mathbb{R}}^{n}$ , let $\|x\|\triangleq\sqrt{x^{\rm T}x}$ .For $x\in{\mathbb{R}}^{n}$ and positive-semidefinite $R\in{\mathbb{R}}^{n\times n}$ , $\|x\|_{R}\triangleq\sqrt{x^{\rm T}Rx}$ .

II Background Material

II-A The one-step Kalman Filter

Consider the discrete-time, linear, time-varying system

	$\displaystyle x_{k+1}$	$\displaystyle=A_{k}x_{k}+B_{k}u_{k}+w_{k},$		(1)
	$\displaystyle y_{k}$	$\displaystyle=C_{k}x_{k}+v_{k},$		(2)

where, for all $k\geq 0$ , $x_{k}\in{\mathbb{R}}^{n}$ is the state, $y_{k}\in{\mathbb{R}}^{p}$ is the measurement, $u_{k}\in{\mathbb{R}}^{m}$ is the input, $w_{k}\sim\mathcal{N}(0,\Sigma_{k})$ is the process noise, and $v_{k}\sim\mathcal{N}(0,\Gamma_{k})$ is the measurement noise, for positive-semidefinite $\Sigma_{k}\in{\mathbb{R}}^{n\times n}$ and positive-semidefinite $\Gamma_{k}\in{\mathbb{R}}^{p\times p}$ .Moreover, for all $k\geq 0$ , $A_{k}\in{\mathbb{R}}^{n\times n}$ , $B_{k}\in{\mathbb{R}}^{n\times m}$ , and $C_{k}\in{\mathbb{R}}^{p\times n}$ .The two-step Kalman filter [20] for the system given by (1) and (2) is can be expressed as

$\displaystyle\hat{x}_{k+1\|k}$	$\displaystyle=A_{k}\hat{x}_{k}+B_{k}u_{k},$	(3)
$\displaystyle P_{k+1\|k}$	$\displaystyle=A_{k}P_{k}A_{k}^{\rm T}+\Sigma_{k},$	(4)
$\displaystyle K_{k}$	$\displaystyle=P_{k+1\|k}C_{k}^{\rm T}(C_{k}P_{k+1\|k}C_{k}^{\rm T}+\Gamma_{k})^{%-1}$	(5)
$\displaystyle\hat{x}_{k+1}$	$\displaystyle=\hat{x}_{k+1\|k}+K_{k}(y_{k}-C_{k}\hat{x}_{k+1\|k}),$	(6)
$\displaystyle P_{k+1}$	$\displaystyle=P_{k+1\|k}-K_{k}C_{k}P_{k+1\|k},$	(7)

where, for all $k\geq 0$ , positive-definite $P_{k+1|k}\in{\mathbb{R}}^{n\times n}$ and $P_{k}\in{\mathbb{R}}^{n\times n}$ are, respectively, the prior and posterior covariances, $\hat{x}_{k+1|k}\in{\mathbb{R}}^{n}$ and $\hat{x}_{k}\in{\mathbb{R}}^{n}$ are, respectively, the prior and posterior state estimates, and $K_{k}\in{\mathbb{R}}^{n\times p}$ is the Kalman gain.

Next, if, for all $k\geq 0$ , $\Sigma_{k}+A_{k}P_{k}A_{k}^{\rm T}$ and $\Gamma_{k}$ are nonsingular, then the matrix inversion lemma (Lemma 1) can be used to rewrite (3) through (7) as the one-step Kalman filter [18], where, for all $k\geq 0$ ,

$\displaystyle P_{k+1}^{-1}=$	$\displaystyle(\Sigma_{k}+A_{k}P_{k}A_{k}^{\rm T})^{-1}+C_{k}^{\rm T}\Gamma_{k}%^{-1}C_{k},$	(8)
$\displaystyle\hat{x}_{k+1}=$	$\displaystyle A_{k}\hat{x}_{k}+B_{k}u_{k}$
	$\displaystyle+P_{k+1}C_{k}^{\rm T}\Gamma_{k}^{-1}\left(y_{k}-C_{k}(A_{k}\hat{x%}_{k}+B_{k}u_{k})\right).$	(9)

II-B Discrete-Time LTV State Transition Function

To facilitate expressing a least squares cost function for the Kalman filter, we first introduce the state transition function for discrete-time LTV system, a concise notation to transition between states at different time steps.For all $k\geq 0$ , let $A_{k}\in{\mathbb{R}}^{n\times n}$ , $B_{k}\in{\mathbb{R}}^{n\times m}$ , $x_{k}\in{\mathbb{R}}^{n}$ , and $u_{k}\in{\mathbb{R}}^{m}$ . Consider the discrete-time, linear time-varying state update equation

\displaystyle x_{k+1}=A_{k}x_{k}+B_{k}u_{k}.

(10)

Assume, for all $k\geq 0$ , $A_{k}$ is nonsingular. For all $i,k\geq 0$ , define the state transition matrix (from step $i$ to step $k$ ), denoted $\Phi_{k,i}\in{\mathbb{R}}^{n\times n}$ , by

\displaystyle\Phi_{k,i}\triangleq\begin{cases}A_{k-1}A_{k-2}\cdots A_{i+1}A_{i%}&i<k,\\I&i=k,\\A_{k}^{-1}A_{k+1}^{-1}\cdots A_{i-2}^{-1}A_{i-1}^{-1}&k<i.\end{cases}

(11)

It follows that, for all $i,k\geq 0$ , $\Phi_{i,k}^{-1}=\Phi_{k,i}$ .Then, for all $0\leq i<k$ , $x_{k}$ can be expressed as

$\displaystyle x_{k}$	$\displaystyle=A_{k-1}x_{k-1}+B_{k-1}u_{k-1},$
	$\displaystyle=A_{k-1}A_{k-2}x_{k-2}+A_{k-1}B_{k-2}u_{k-2}+B_{k-1}u_{k-1},$
	$\displaystyle=\ \cdots\ =\Phi_{k,i}x_{i}+\sum_{j=i}^{k-1}\Phi_{k,j+1}B_{j}u_{j}.$	(12)

For all $0\leq i<k$ , we further define the matrices

	$\displaystyle\mathcal{B}_{k,i}$	$\displaystyle\triangleq\begin{bmatrix}\Phi_{k,{i+1}}B_{i}&\cdots&\Phi_{k,{k-1}%}B_{k-2}&\Phi_{k,{k}}B_{k-1}\end{bmatrix},$		(14)
	$\displaystyle\mathcal{U}_{k,i}$	$\displaystyle\triangleq\begin{bmatrix}u_{i}^{\rm T}&\cdots&u_{k-2}^{\rm T}&u_{%k-1}^{\rm T}\end{bmatrix}^{\rm T}.$		(16)

It follows that $\mathcal{B}_{k,i}\mathcal{U}_{k,i}=\sum_{j=i}^{k-1}\Phi_{k,j+1}B_{j}u_{j}$ .Hence, for all $0\leq i<k$ , $x_{k}=\Phi_{k,i}x_{i}+\mathcal{B}_{k,i}\mathcal{U}_{k,i}$ .On the other hand, for all $0\leq k<i$ , $x_{k}=\Phi_{k,i}(x_{i}-\mathcal{B}_{i,k}\mathcal{U}_{i,k})$ .Therefore, for all $i\geq 0$ and $k\geq 0$ , we define the state transition function (from step $i$ to step $k$ ), denoted $\mathcal{T}_{k,i}:{\mathbb{R}}^{n}\rightarrow{\mathbb{R}}^{n}$ , by

\displaystyle\mathcal{T}_{k,i}(x)\triangleq\begin{cases}\Phi_{k,i}x+\mathcal{B%}_{k,i}\mathcal{U}_{k,i}&i<k,\\x&i=k,\\\Phi_{k,i}(x-\mathcal{B}_{i,k}\mathcal{U}_{i,k})&k<i.\end{cases}

(17)

III A Least Squares Cost Function which Derives the Kalman Filter

This section develops the Kalman filter least squares (KFLS) cost function whose recursive minimizer gives the one-step Kalman filter update equations.To begin, for all $k\geq 0$ , let $F_{k}\in{\mathbb{R}}^{n\times n}$ be the forgetting matrix.Theorem 1 develops the KFLS cost function (19) in terms of $F_{k}$ and shows how the update equations (23) and (24) minimize that cost.Corollary 2 will later show how, for a particular choice of $F_{k}$ , the update equations of Theorem 1 are equivalent to the one-step Kalman filter.

Theorem 1.

For all $k\geq 0$ , let $A_{k}\in{\mathbb{R}}^{n\times n}$ be nonsingular, $B_{k}\in{\mathbb{R}}^{n\times m}$ , $C_{k}\in{\mathbb{R}}^{p\times n}$ , $\Gamma_{k}\in{\mathbb{R}}^{p\times p}$ be positive definite, $u_{k}\in{\mathbb{R}}^{m}$ , and $y_{k}\in{\mathbb{R}}^{p}$ . Furthermore, let $P_{0}\in{\mathbb{R}}^{n\times n}$ be positive definite and $\hat{x}_{0}\in{\mathbb{R}}^{n}$ . For all $k\geq 0$ , Let $F_{k}\in{\mathbb{R}}^{n\times n}$ be positive semidefinite and such that

	$\displaystyle F_{k}$	$\displaystyle\prec\Phi_{0,k}^{\rm T}P_{0}^{-1}\Phi_{0,k}$
		$\displaystyle+\sum_{i=0}^{k-1}\left(\Phi_{i+1,k}^{\rm T}C_{i}^{\rm T}\Gamma_{i%}^{-1}C_{i}\Phi_{i+1,k}-\Phi_{i,k}^{\rm T}F_{i}\Phi_{i,k}\right).$		(18)

For all $k\geq 0$ , define $J_{k}\colon{\mathbb{R}}^{n}\rightarrow{\mathbb{R}}$ as

\displaystyle J_{k}(\hat{x})\triangleq J_{k,{\rm loss}}(\hat{x})-J_{k,{\rmforget%}}(\hat{x})+J_{k,{\rm reg}}(\hat{x}),

(19)

where

$\displaystyle J_{k,{\rm loss}}(\hat{x})$	$\displaystyle\triangleq\sum_{i=0}^{k}\\|y_{i}-C_{i}\mathcal{T}_{i+1,k+1}(\hat{x%})\\|_{\Gamma_{i}^{-1}}^{2}$	(20)
$\displaystyle J_{k,{\rm forget}}(\hat{x})$	$\displaystyle\triangleq\sum_{i=0}^{k}\\|\mathcal{T}_{i,k+1}(\hat{x})-\hat{x}_{i%}\\|_{F_{i}}^{2},$	(21)
$\displaystyle J_{k,{\rm reg}}(\hat{x})$	$\displaystyle\triangleq\\|\mathcal{T}_{0,k+1}(\hat{x})-\hat{x}_{0}\\|_{P_{0}^{-1%}}^{2}.$	(22)

Then, there exists a unique global minimizer of $J_{k}(\hat{x})$ , denoted $\hat{x}_{k+1}\triangleq\operatorname*{arg\,min}_{\hat{x}\in{\mathbb{R}}^{n}}J_%{k}(\hat{x})$ ,which, for all $k\geq 0$ , is given recursively by

Proof.

See the Appendix.∎

Corollary 1.

Consider the notation and assumptions of Theorem 1.For all $k\geq 0$ ,

	$\displaystyle P_{k}^{-1}$	$\displaystyle=\Phi_{0,k}^{\rm T}P_{0}^{-1}\Phi_{0,k}$
		$\displaystyle+\sum_{i=0}^{k-1}\left(\Phi_{i+1,k}^{\rm T}C_{i}^{\rm T}\Gamma_{i%}^{-1}C_{i}\Phi_{i+1,k}-\Phi_{i,k}^{\rm T}F_{i}\Phi_{i,k}\right),$		(25)

and hence (18) hold if and only if $P_{k}^{-1}-F_{k}\succ 0$ .

Proof.

Let $k\geq 0$ . Note that (25) follows from repeated substitution of (23).Next, note that the right-hand side of (25) is equivalent to the right-hand side of (18).Hence, (18) is equivalent to $P_{k}^{-1}-F_{k}\succ 0$ ∎

Corollary 2.

Consider the notation and assumptions of Theorem 1.If, for all $k\geq 0$ , there exists positive-semidefinite $\Sigma_{k}\in{\mathbb{R}}^{n\times n}$ such that

\displaystyle F_{k}=P_{k}^{-1}-(A_{k}^{-1}\Sigma_{k}A_{k}^{-{\rm T}}+P_{k})^{-%1},

(26)

then, for all $k\geq 0$ , (18) is satisfied. Moreover, (23) and (24) are equivalent to the one-step Kalman filter update equations (8) and (9).

Proof.

Let $k\geq 0$ . Note that (26) can be rewritten as

\displaystyle(\Sigma_{k}+A_{k}P_{k}A_{k}^{\rm T})^{-1}=A_{k}^{-{\rm T}}(P_{k}^%{-1}-F_{k})A_{k}^{-1}.

(27)

Substituting (27) into (23), it follows that (23) and (24) are equivalent to (8) and (9).Moreover, (26) can also be expressed as $P_{k}^{-1}-F_{k}=(A_{k}^{-1}\Sigma_{k}A_{k}^{-{\rm T}}+P_{k})^{-1}$ .Since $\Sigma_{k}$ and $P_{k}$ are both positive definite, it follows that $A_{k}^{-1}\Sigma_{k}A_{k}^{-{\rm T}}+P_{k}$ is positive definite, and hence $P_{k}^{-1}-F_{k}$ is positive definite.Therefore, by Corollary 1, condition (18) is satisfied.∎

Next, Proposition 1 shows that, for all $k\geq 0$ , $\Sigma_{k}$ has the same definiteness as $F_{k}$ .

Proposition 1.

For all $k\geq 0$ , $\Sigma_{k}$ is positive semidefinite (resp. positive definite) if and only on $F_{k}$ is positive semidefinite (resp. positive definite).

Proof.

Let $k\geq 0$ .If $F_{k}$ is positive semidefinite (respectively, positive definite), then $P_{k}^{-1}-F_{k}\preceq P_{k}^{-1}$ (respectively, $P_{k}^{-1}-F_{k}\prec P_{k}^{-1}$ ).Since $P_{k}^{-1}-F_{k}$ is nonsingular by Corollary 1, it follows that $(P_{k}^{-1}-F_{k})^{-1}\succeq P_{k}$ and hence $(P_{k}^{-1}-F_{k})^{-1}-P_{k}\succeq 0$ (respectively, $(P_{k}^{-1}-F_{k})^{-1}-P_{k}\succ 0$ ).Finally, since $A_{k}$ is nonsingular by assumption, it follows from (31) that $\Sigma_{k}$ is positive semidefinite (respectively, positive definite).

Next, if $\Sigma_{k}$ is positive semidefinite (respectively, positive definite), then $\Sigma_{k}+P_{k}\succeq P_{k}$ (respectively, $\Sigma_{k}+P_{k}\succ P_{k}$ ) and hence $(\Sigma_{k}+P_{k})^{-1}\preceq P_{k}^{-1}$ (respectively, $(\Sigma_{k}+P_{k})^{-1}\prec P_{k}^{-1}$ ).Therefore, $F_{k}=P_{k}^{-1}-(\Sigma_{k}+P_{k})^{-1}\succeq 0$ (respectively, $F_{k}=P_{k}^{-1}-(\Sigma_{k}+P_{k})^{-1}\succ 0$ ).∎

IV RLS Extensions as Special Cases of the Kalman Filter

Revisiting the state update equation (10), note that if, for all $k\geq 0$ , $A_{k}=I_{n}$ and $B_{k}=0_{n\times m}$ , then, for all $k\geq 0$ , $x_{k+1}=x_{k}$ .This also implies that the state transition function, given by (17), is identity.In particular, for all $i\geq 0$ , $k\geq 0$ , and $x\in{\mathbb{R}}^{n}$ , $\mathcal{T}_{k,i}(x)=x$ .Substituting the identity state transition function into the cost function $J_{k}$ given by (19), it follows that, for all $k\geq 0$ ,

$\displaystyle J_{k,{\rm loss}}(\hat{x})$	$\displaystyle=\sum_{i=0}^{k}\\|y_{i}-C_{i}\hat{x}\\|_{\Gamma_{i}^{-1}}^{2}$	(28)
$\displaystyle J_{k,{\rm forget}}(\hat{x})$	$\displaystyle=\sum_{i=0}^{k}\\|\hat{x}-\hat{x}_{i}\\|_{F_{i}}^{2},$	(29)
$\displaystyle J_{k,{\rm reg}}(\hat{x})$	$\displaystyle=\\|\hat{x}-\hat{x}_{0}\\|_{P_{0}^{-1}}^{2}.$	(30)

Note that, in this special case, the cost function $J_{k}$ is equivalent to the generalized forgetting recursive least squares (GF-RLS) cost developed in [19], where GF-RLS uses the notation $\theta\in{\mathbb{R}}^{n}$ and $\phi_{k}\in{\mathbb{R}}^{p\times n}$ instead of $\hat{x}\in{\mathbb{R}}^{n}$ and $C_{k}\in{\mathbb{R}}^{p\times n}$ , respectively.In [19], it was shown that various extensions of RLS from the literature are special cases of GF-RLS if, for all $k\geq 0$ , a particular forgetting matrix $F_{k}\in{\mathbb{R}}^{n\times n}$ is chosen.

Therefore, we conclude that an extension of RLS, which is a special case of GF-RLS with forgetting matrix $F_{k}$ , $k\geq 0$ , is also a special case of the Kalman filter if, for all $k\geq 0$ , $A_{k}=I_{n}$ , $B_{k}=0_{n\times m}$ , and there exists positive-semidefinite $\Sigma_{k}\in{\mathbb{R}}^{n\times n}$ such that (26) holds.Explicitly solving for $\Sigma_{k}$ , it follows that

\displaystyle\Sigma_{k}=A_{k}\left[(P_{k}^{-1}-F_{k})^{-1}-P_{k}\right]A_{k}^{%\rm T},

(31)

where $P_{k}^{-1}-F_{k}$ is nonsingular by (18) and Corollary 1.Moreover, by Proposition 1, $\Sigma_{k}$ is positive semidefinite if and only if $F_{k}$ is positive semidefinite.

While [19] gives a thorough literature review on extensions of RLS as special cases of GF-RLS, for brevitiy, we summarize eight extensions in Table I.Given are the algorithm name and reference, assumptions of the algorithm, tuning parameters, and $\Sigma_{k}\in{\mathbb{R}}^{n\times n}$ derived from (31).

Algorithm

Tuning Parameters

Process Noise Covariance

\Sigma_{k}

1. Recursive Least Squares [2]

—

\Sigma_{k}=0_{n\times n}

2. Exponential Forgetting [2, 21]

\lambda\in(0,1]

\Sigma_{k}=(\frac{1}{\lambda}-1)P_{k}.

3. Variable-Rate Forgetting [7]

\lambda_{k}\in(0,1]

k\geq 0

\Sigma_{k}=(\frac{1}{\lambda_{k}}-1)P_{k}.

4. Data-Dependent Forgetting [16]

\mu_{-1}=1

and

\mu_{k}\in[0,1),k\geq 0

\Sigma_{k}=(\frac{\mu_{k}}{(1-\mu_{k})\mu_{k-1}}-1)P_{k}.

5. Exponential Resetting [12]

positive-definite

P_{\infty}\in{\mathbb{R}}^{n\times n}

\Sigma_{k}=(\lambda P_{k}^{-1}+(1-\lambda)P_{\infty}^{-1})^{-1}-P_{k}.

6. Covariance Resetting [22]

positive-definite

P_{\infty}\in{\mathbb{R}}^{n\times n}

and resetting criteria

\Sigma_{k}=\begin{cases}P_{\infty}-P_{k}&\textnormal{criteria met},\\0_{n\times n}&\textnormal{otherwise}.\end{cases}

7. Directional Forgetting by

Information Matrix Decomp. [8]

\lambda\in(0,1]

\Sigma_{k}=\frac{1-\lambda}{\lambda}C_{k}^{\rm T}(C_{k}P_{k}^{-1}C_{k}^{\rm T}%)^{-1}C_{k}.

8. Variable-Direction Forgetting [21]

Positive-definite

\Lambda_{k}\in{\mathbb{R}}^{n\times n}

k\geq 0

\Sigma_{k}=\Lambda_{k}^{-1}P_{k}^{-1}\Lambda_{k}^{-1}-P_{k}.

V Adaptive Kalman Filtering Developed from RLS Forgetting Algorithms

Thus far, we have shown that various extensions of RLS from the literature are special cases of the Kalman filter, in part, by a special choice of the process noise covariance given by (31).Motivated by this relationship, we propose a class of adaptive Kalman filters by replacing the prior covariance update equation (4) with the adaptive prior covariance update equations

	$\displaystyle P_{{\rm forget},k}$	$\displaystyle=P_{k}+\Sigma_{{\rm forget},k},$		(32)
	$\displaystyle P_{k+1\|k}$	$\displaystyle=A_{k}P_{{\rm forget},k}A_{k}^{\rm T}+\Sigma_{{\rm Kalman},k},$		(33)

where, for all $k\geq 0$ , positive-semidefinite $\Sigma_{{\rm forget},k}\in{\mathbb{R}}^{n\times n}$ is designed from an extension of RLS (e.g. right column of Table I), and positive-semidefinite $\Sigma_{{\rm Kalman},k}\in{\mathbb{R}}^{n\times n}$ is designed using traditional methods of the Kalman filter.

While there are as many variants of this algorithm as extensions of RLS, we select a particular extension of RLS to show the potential benefits in state estimation.

V-A Kalman Filter with Robust Variable Forgetting Factor

Consider the mass-spring-damper system in Figure 1, with mass $m=10$ , spring constant $k=5$ , and damping coefficient $c=3$ .The vertical displacement of the mass at time $t$ is $z(t)$ , where $z=0$ when the mass is at rest.Assume that $z(0)=-1$ and $\dot{z}(0)=1$ Furthermore, a downward force $F(t)=10\sin(t)$ is applied on the mass at time $t$ .This nominal mass-spring-damper system can be modeled as

\displaystyle\begin{bmatrix}\dot{z}(t)\\\ddot{z}(t)\end{bmatrix}=\begin{bmatrix}0&1\\-\nicefrac{{k}}{{m}}&-\nicefrac{{b}}{{m}}\end{bmatrix}\begin{bmatrix}z(t)\\\dot{z}(t)\end{bmatrix}+\begin{bmatrix}0\\\nicefrac{{1}}{{m}}\end{bmatrix}F(t).

(42)

However, we additionally assume there is a wall at $z=2$ . If the mass collides with this wall, the mass reverses direction with the same speed.Note that such intermittent collisions can be interpreted as impulsive disturbances on the system.Finally, we consider, for all $k\geq 0$ , the measurements $y_{k}\in{\mathbb{R}}$ , given by

\displaystyle y_{k}=z(kT_{s})+\dot{z}(kT_{s})+v_{k},

(43)

where $T_{s}=0.1$ is the sampling time and, for all $k\geq 0$ , $v_{k}\sim\mathcal{N}(0,\Gamma_{k})$ is the measurement noise with $\Gamma_{k}=0.01$ .The goal is to estimate the vertical displacement $z(t)$ and velocity $\dot{z}(t)$ without knowledge of the wall at $z=2$ .We will assume that the nominal model (42) and the measurement noise covariance $\Gamma_{k}$ are known.

Adaptive Kalman Filtering Developed from Recursive Least Squares Forgetting Algorithms (1)

We begin by discretizing (42) and (43) using zero-order hold and sampling time $T_{s}$ to obtain the nominal system

	$\displaystyle x_{k+1}$	$\displaystyle=A_{k}x_{k}+B_{k}u_{k},$
	$\displaystyle y_{k}$	$\displaystyle=C_{k}x_{k}+v_{k},$		(44)

where, for all $k\geq 0$ , $x_{k}\triangleq[z(kT_{s})\ \dot{z}(kT_{s})]^{\rm T}$ , $u_{k}\triangleq F(kT_{s})$ ,

	$\displaystyle A_{k}$	$\displaystyle\triangleq\begin{bmatrix}0.9975&0.09843\\-0.04922&0.9680\end{bmatrix},\ B_{k}\triangleq\begin{bmatrix}$4.948\text{%\times}{10}^{-4}$\\$9.843\text{\times}{10}^{-3}$\end{bmatrix},$		(49)
	$\displaystyle C_{k}$	$\displaystyle\triangleq\begin{bmatrix}1&1\end{bmatrix}.$		(51)

We first consider state estimation using the Kalman filter with the nominal discrete system (44), and $P_{0}=0.1I_{2}$ , $\hat{x}_{0}=[0\ 0]^{\rm T}$ , and, for all $k\geq 0$ , $\Gamma_{k}=0.01$ , $\Sigma_{k}=0.01I_{2}$ .

Second, we will consider an adaptive Kalman filter with adaptive prior covariance update equations (32) and (33) developed from variable-rate forgetting [7].This adaptive Kalman filter also uses the nominal discrete system (44), and $P_{0}=0.1I_{2}$ , $\hat{x}_{0}=[0\ 0]^{\rm T}$ , and, for all $k\geq 0$ , $\Gamma_{k}=0.01$ .Then, for all $k\geq 0$ , let

\displaystyle\Sigma_{{\rm Kalman},k}=0.01I_{2},\quad\Sigma_{{\rm forget},k}=(%\frac{1}{\lambda_{k}}-1)P_{k},

(52)

where, the forgetting factor $\lambda_{k}\in(0,1]$ is chosen using the robust variable forgetting factor algorithm developed in [14].We’ve chosen this algorithm for its ability to improve tracking of impulsive changes of parameters [14].For all $k\geq 0$ , let

$\displaystyle\hat{\sigma}_{e,k}^{2}$	$\displaystyle=\alpha\hat{\sigma}_{e,k-1}^{2}+(1-\alpha)(y_{k}-C_{k}\hat{x}_{k}%)^{2},$	(53)
$\displaystyle\hat{\sigma}_{q,k}^{2}$	$\displaystyle=\alpha\hat{\sigma}_{q,k-1}^{2}+(1-\alpha)(\hat{x}_{k}^{\rm T}P_{%k}\hat{x}_{k})^{2},$	(54)
$\displaystyle\hat{\sigma}_{v,k}^{2}$	$\displaystyle=\beta\hat{\sigma}_{v,k-1}^{2}+(1-\beta)(y_{k}-C_{k}\hat{x}_{k})^%{2},$	(55)

where $\hat{\sigma}_{e,-1}\triangleq\hat{\sigma}_{q,-1}\triangleq\hat{\sigma}_{v,-1}\triangleq1$ , $\alpha\triangleq 1-\frac{1}{K_{\alpha}n}$ , and $\beta\triangleq 1-\frac{1}{K_{\beta}n}$ , where $K_{\alpha}\triangleq 2$ , $K_{\beta}\triangleq 10$ , and $n=2$ since the system is second-order.

Then, for all $k\geq 0$ , if $\hat{\sigma}_{e,k}\leq\hat{\sigma}_{v,k}$ , $\lambda_{k}=\lambda_{\rm max}$ , otherwise

\displaystyle\lambda_{k}=\max\left\{\min\left\{\frac{\hat{\sigma}_{q,k}\hat{%\sigma}_{v,k}}{\xi+|\hat{\sigma}_{e,k}-\hat{\sigma}_{v,k}|},\lambda_{\rm max}%\right\},\lambda_{\rm min}\right\},

(56)

where $\xi\triangleq 10^{-6}$ , $\lambda_{\rm min}\triangleq 0.5$ , and $\lambda_{\rm max}\triangleq 1$ .For details on robust variable forgetting and tuning of parameters, see [14].

Figure 2 shows state estimation of the Kalman filter (KF) and the adaptive Kalman filter (KF*), as well as the forgetting factor $\lambda_{k}$ , all with zero-order hold.Note that after each of the four collisions the mass makes with the wall at $z=2$ , the forgetting factor $\lambda_{k}$ briefly but drastically decreases.This results in improved displacement and velocity estimation immediately after each collision.This can be more clearly seen in the error between the true state and the estimated state in Figure 3.Figure 4 shows $\sigma_{\hat{z}}$ and $\sigma_{\hat{\dot{z}}}$ , the diagonal elements of the covariance matrix $P_{k}$ , which can also be interpreted as the marginal variance of the displacement and velocity state estimate, respectively.Note that in the adaptive Kalman filter (KF*), there is a sudden increase in both $\sigma_{\hat{z}}$ and $\sigma_{\hat{\dot{z}}}$ after each collision, allowing for quicker adaptation.

Adaptive Kalman Filtering Developed from Recursive Least Squares Forgetting Algorithms (2)

Adaptive Kalman Filtering Developed from Recursive Least Squares Forgetting Algorithms (3)

Adaptive Kalman Filtering Developed from Recursive Least Squares Forgetting Algorithms (4)

VI Conclusion

This work presents the Kalman filter least squares cost function whose recursive minimizer gives the Kalman filter update equations.An important consequence of this cost function is that various extensions of RLS from the literature are special cases of the Kalman filter.Motivated by this result, we propose a new adaptive Kalman filters, whose prior covariance update is modified to include RLS forgetting.While the numerical example we presented shows the potential benefits of adaptive Kalman filtering with robust variable forgetting factor [14] in the presence of impulsive disturbances, there are numerous other forgetting algorithms in the RLS literature to be considered (several summarized in Table I).Future work includes further exploration into how, and in what situations, such extensions may be beneficial.

References

[1]H.W. Sorenson, “Least-squares estimation: from gauss to kalman,” IEEE Spectrum, vol.7, no.7, pp. 63–68, 1970.
[2]S.Islam and D.Bernstein, “Recursive least squares for real-time implementation,” IEEE Ctrl. Sys. Mag., vol.39, no.3, p.82, 2019.
[3]K.J. Åström and B.Wittenmark, Adaptive control.Courier Co., 2013.
[4]J.L. Crassidis and J.L. Junkins, Optimal estimation of dynamic systems.Chapman and Hall/CRC, 2004.
[5]D.Simon, Optimal state estimation: Kalman, H infinity, and nonlinear approaches.John Wiley & Sons, 2006.
[6]L.Ljung and T.Söderström, Theory and practice of recursive identification.MIT press, 1983.
[7]A.L. Bruce, A.Goel, and D.S. Bernstein, “Convergence and consistency of recursive least squares with variable-rate forgetting,” Automatica, vol. 119, p. 109052, 2020.
[8]L.Cao and H.Schwartz, “A directional forgetting algorithm based on the decomposition of the information matrix,” Automatica, vol.36, no.11, pp. 1725–1731, 2000.
[9]R.Kulhavỳ and M.Kárnỳ, “Tracking of slowly varying parameters by directional forgetting,” IFAC Proceedings Volumes, vol.17, no.2, pp. 687–692, 1984.
[10]S.Bittanti, P.Bolzern, and M.Campi, “Convergence and exponential convergence of identification algorithms with directional forgetting factor,” Automatica, vol.26, no.5, pp. 929–932, 1990.
[11]M.E. Salgado, G.C. Goodwin, and R.H. Middleton, “Modified least squares algorithm incorporating exponential resetting and forgetting,” International Journal of Control, vol.47, no.2, pp. 477–491, 1988.
[12]B.Lai and D.S. Bernstein, “Exponential resetting and cyclic resetting recursive least squares,” IEEE Ctrl. Sys. Lets., vol.7, pp. 985–990, 2023.
[13]A.Vahidi etal., “Recursive least squares with forgetting for online estimation of vehicle mass and road grade: theory and experiments,” Vehicle Sys. Dynms., vol.43, no.1, pp. 31–55, 2005.
[14]C.Paleologu, J.Benesty, and S.Ciochina, “A robust variable forgetting factor recursive least-squares algorithm for system identification,” IEEE Signal Processing Letters, vol.15, pp. 597–600, 2008.
[15]R.M. Johnstone, C.R. JohnsonJr, R.R. Bitmead, and B.D. Anderson, “Exponential convergence of recursive least squares with exponential forgetting factor,” Sys. & Ctrl. Letters, vol.2, no.2, pp. 77–82, 1982.
[16]S.Dasgupta and Y.-F. Huang, “Asymptotically convergent modified recursive least-squares with data-dependent updating and forgetting factor for systems with bounded noise,” IEEE Transactions on Information Theory, vol.33, no.3, pp. 383–392, 1987.
[17]J.Humpherys and J.West, “Kalman filtering with newton’s method,” IEEE Control Systems Magazine, vol.30, no.6, pp. 101–106, 2010.
[18]J.Humpherys, P.Redd, and J.West, “A fresh look at the kalman filter,” SIAM review, vol.54, no.4, pp. 801–823, 2012.
[19]B.Lai and D.S. Bernstein, “Generalized forgetting recursive least squares: Stability and robustness guarantees,” 2023.
[20]R.E. Kalman, “A new approach to linear filtering and prediction problems,” Transactions of the ASME–Journal of Basic Engineering, vol.82, no. Series D, pp. 35–45, 1960.
[21]A.Goel, A.L. Bruce, and D.S. Bernstein, “Recursive least squares with variable-direction forgetting: Compensating for the loss of persistency,” IEEE Ctrl. Sys. Mag., vol.40, no.4, pp. 80–102, 2020.
[22]G.Goodwin, E.Teoh, and H.Elliott, “Deterministic convergence of a self-tuning regulator with covariance resetting,” in IEE Proceedings D-Control Theory and Applications, vol.1, no. 130, 1983, pp. 6–8.

Appendix

Lemma 1.

Let $A\in{\mathbb{R}}^{n\times n}$ , $U\in{\mathbb{R}}^{n\times p}$ , $C\in{\mathbb{R}}^{p\times p}$ , $V\in{\mathbb{R}}^{p\times n}$ . Assume $A$ , $C$ , and $A+UCV$ are nonsingular. Then, $(A+UCV)^{-1}=A^{-1}-A^{-1}U(C^{-1}+VA^{-1}U)^{-1}VA^{-1}$ .

Lemma 2.

Let $A\in{\mathbb{R}}^{n\times n}$ be positive definite, let $b\in{\mathbb{R}}^{n}$ and $c\in{\mathbb{R}}$ , and define $f\colon{\mathbb{R}}^{n}\rightarrow{\mathbb{R}}$ by $f(x)\triangleq x^{\rm T}Ax+2b^{\rm T}x+c$ .Then, $f$ has a unique stationary point, which is the global minimizer given by $\operatorname*{arg\,min}_{x\in{\mathbb{R}}^{n}}f(x)=-A^{-1}b$ .

Proof of Theorem 1.

We write $J_{0}(\hat{x})$ , given by (19), as

	$\displaystyle J_{0}(\hat{x})=\\|y_{0}-C_{0}\hat{x}\\|_{\Gamma_{0}^{-1}}^{2}+\\|A_%{0}^{-1}(\hat{x}-B_{0}u_{0})-\hat{x}_{0}\\|_{P_{0}^{-1}-F_{0}}^{2},$
	$\displaystyle=\\|y_{0}-C_{0}\hat{x}\\|_{\Gamma_{0}^{-1}}^{2}+\\|\hat{x}-(A_{0}%\hat{x}_{0}+B_{0}u_{0})\\|_{A_{0}^{-{\rm T}}(P_{0}^{-1}-F_{0})A_{0}^{-1}}^{2}$

Next, we expand $J_{0}(\hat{x})=\hat{x}^{\rm T}H_{0}\hat{x}+2b_{0}^{\rm T}\hat{x}+c_{0}$ , where

$\displaystyle H_{0}$	$\displaystyle\triangleq C_{0}^{\rm T}\Gamma_{0}^{-1}C_{0}+A_{0}^{-{\rm T}}(P_{%0}^{-1}-F_{0})A_{0}^{-1}$	(57)
$\displaystyle b_{0}$	$\displaystyle\triangleq-C_{0}^{\rm T}\Gamma_{0}^{-1}y_{0}-A_{0}^{-{\rm T}}(P_{%0}^{-1}-F_{0})A_{0}^{-1}(A_{0}\hat{x}_{0}+B_{0}u_{0})$
$\displaystyle c_{0}$	$\displaystyle\triangleq y_{0}^{\rm T}\Gamma_{0}^{-1}y_{0}+\\|A_{0}^{-1}(A_{0}%\hat{x}_{0}+B_{0}u_{0})\\|_{P_{0}^{-1}-F_{0}}^{2}$

Defining $P_{1}\triangleq H_{0}^{-1}$ , it follows from the expression of $H_{0}$ that (23) is satisfied for $k=0$ .Furthermore, if $k=0$ , (18) simplifies to $F_{0}\prec P_{0}^{-1}$ and hence $P_{0}^{-1}-F_{0}\succ 0$ .Furthermore, since $\Gamma_{0}$ is positive definite, it follows from definition (57) that $H_{0}$ is positive definite.Therefore, from Lemma 2, it follows that the unique minimizer $\hat{x}_{1}$ of $J_{0}$ is given by $\hat{x}_{1}=-H_{0}^{-1}b_{0}$ , which can be expanded as

	$\displaystyle\hat{x}_{1}=$	$\displaystyle P_{1}\big{[}C_{0}^{\rm T}\Gamma_{0}^{-1}y_{0}+A_{0}^{-{\rm T}}(P%_{0}^{-1}-F_{0})A_{0}^{-1}(A_{0}\hat{x}_{0}+B_{0}u_{0})\big{]}$
	$\displaystyle=$	$\displaystyle P_{1}\big{[}C_{0}^{\rm T}\Gamma_{0}^{-1}y_{0}-C_{0}^{\rm T}%\Gamma_{0}^{-1}C_{0}(A_{0}\hat{x}_{0}+B_{0}u_{0})$
		$\displaystyle+(A_{0}^{-{\rm T}}(P_{0}^{-1}-F_{0})A_{0}^{-1}+C_{0}^{\rm T}%\Gamma_{0}^{-1}C_{0})(A_{0}\hat{x}_{0}+B_{0}u_{0})\big{]}$
	$\displaystyle=$	$\displaystyle P_{1}\big{[}C_{0}^{\rm T}\Gamma_{0}^{-1}y_{0}-C_{0}^{\rm T}%\Gamma_{0}^{-1}C_{0}(A_{0}\hat{x}_{0}+B_{0}u_{0})$
		$\displaystyle+P_{1}^{-1}(A_{0}\hat{x}_{0}+B_{0}u_{0})\big{]}$
	$\displaystyle=$	$\displaystyle A_{0}\hat{x}_{0}+B_{0}u_{0}+P_{1}C_{0}^{\rm T}\Gamma_{0}^{-1}(y_%{0}-C_{0}(A_{0}\hat{x}_{0}+B_{0}u_{0})).$

Hence, (24) holds for $k=0$ . Moreover, since $H_{0}$ is positive definite, it follows that $P_{1}=H_{0}^{-1}$ is also positive definite.

Now, let $k\geq 1$ .Note that for all $0\leq i\leq k$ ,

	$\displaystyle(\mathcal{T}_{i,k+1}(\hat{x})-\hat{x}_{i})$	$\displaystyle=\Phi_{i,k+1}(\hat{x}-\mathcal{B}_{k+1,i}\mathcal{U}_{k+1,i})-%\hat{x}_{i}$
		$\displaystyle=\Phi_{i,k+1}(\hat{x}-(\Phi_{k+1,i}\hat{x}_{i}+\mathcal{B}_{k+1,i%}\mathcal{U}_{k+1,i}))$
		$\displaystyle=\Phi_{i,k+1}(\hat{x}-\mathcal{T}_{k+1,i}(\hat{x}_{i})).$

Hence, $J_{k}(\hat{x})$ , given by (19), can be written as

	$\displaystyle J_{k}(\hat{x})=\\|\Phi_{0,k+1}(\hat{x}-\mathcal{T}_{k+1,0}(\hat{x%}_{0}))\\|_{P_{0}^{-1}}^{2}+$
	$\displaystyle\sum_{i=0}^{k}\\|y_{i}-C_{i}\mathcal{T}_{i+1,k+1}(\hat{x})\\|_{%\Gamma_{i}^{-1}}^{2}-\\|\Phi_{i,k+1}(\hat{x}-\mathcal{T}_{k+1,i}(\hat{x}_{i}))%\\|_{F_{i}}.$

Next, we expand $J_{k}(\hat{x})=\hat{x}^{\rm T}H_{k}\hat{x}+2b_{k}^{\rm T}\hat{x}+c_{k}$ , where

	$\displaystyle H_{k}\triangleq\sum_{i=0}^{k}\Big{[}\Phi_{i+1,k+1}^{\rm T}C_{i}^%{\rm T}\Gamma_{i}^{-1}C_{i}\Phi_{i+1,k+1}-\Phi_{i,k+1}^{\rm T}F_{i}\Phi_{i,k+1%}\Big{]}$
	$\displaystyle\hskip 10.0pt+\Phi_{0,k+1}^{\rm T}P_{0}^{-1}\Phi_{0,k+1},$		(58)
	$\displaystyle b_{k}\triangleq\sum_{i=0}^{k}\Big{[}\Phi_{i,k+1}^{\rm T}F_{i}%\Phi_{i,k+1}\mathcal{T}_{k+1,i}(\hat{x}_{i})$
	$\displaystyle\hskip 10.0pt-\Phi_{i+1,k+1}^{\rm T}C_{i}^{\rm T}\Gamma_{i}^{-1}(%y_{i}+C_{i}\Phi_{i+1,k+1}\mathcal{B}_{k+1,i+1}\mathcal{U}_{k+1,i+1})\Big{]}$
	$\displaystyle\hskip 10.0pt-\Phi_{0,k+1}^{\rm T}P_{0}^{-1}\Phi_{0,k+1}\mathcal{%T}_{k+1,0}(\hat{x}_{0}),$		(59)
	$\displaystyle c_{k}\triangleq\sum_{i=0}^{k}\Big{[}\\|y_{i}+C_{i}\Phi_{i+1,k+1}%\mathcal{B}_{k+1,i+1}\mathcal{U}_{k+1,i+1}\\|_{\Gamma_{i}}^{2}$
	$\displaystyle\hskip 10.0pt-\\|\Phi_{i,k+1}\mathcal{T}_{k+1,i}(\hat{x}_{i})\\|_{F%_{i}}^{2}\Big{]}+\\|\Phi_{0,k+1}\mathcal{T}_{k+1,0}(\hat{x}_{i})\\|_{P_{0}^{-1}}%^{2}.$

Note that $H_{k}$ can be written recursively as

$\displaystyle H_{k}=$	$\displaystyle\sum_{i=0}^{k-1}\left[\Phi_{i+1,k+1}^{\rm T}C_{i}^{\rm T}\Gamma_{%i}^{-1}C_{i}\Phi_{i+1,k+1}-\Phi_{i,k+1}^{\rm T}F_{i}\Phi_{i,k+1}\right]$
	$\displaystyle+\Phi_{0,k+1}^{\rm T}P_{0}^{-1}\Phi_{0,k+1}+C_{k}^{\rm T}\Gamma_{%k}^{-1}C_{k}-A_{k}^{-{\rm T}}F_{k}A^{-1}$
$\displaystyle=$	$\displaystyle A_{k}^{-{\rm T}}\Big{[}\sum_{i=0}^{k-1}\left(\Phi_{i+1,k}^{\rm T%}C_{i}^{\rm T}\Gamma_{i}^{-1}C_{i}\Phi_{i+1,k}-\Phi_{i,k}^{\rm T}F_{i}\Phi_{i,%k}\right)$
	$\displaystyle+\Phi_{0,k}^{\rm T}P_{0}^{-1}\Phi_{0,k}\Big{]}A_{k}^{-1}+C_{k}^{%\rm T}\Gamma_{k}^{-1}C_{k}-A_{k}^{-{\rm T}}F_{k}A^{-1}$
$\displaystyle=$	$\displaystyle A_{k}^{-{\rm T}}(H_{k-1}-F_{k})A_{k}^{-1}+C_{k}^{\rm T}\Gamma_{k%}^{-1}C_{k}.$	(60)

Defining $P_{k+1}\triangleq H_{k}^{-1}$ ,it follows that (23) is satisfied.

Next, to write a recursive update for $b_{k}$ , we first write $b_{k}$ as $b_{k}=b_{k,1}+b_{k,2}+b_{k,3}$ , where

	$\displaystyle b_{k,1}\triangleq\sum_{i=0}^{k-1}\Big{[}\Phi_{i,k+1}^{\rm T}F_{i%}\Phi_{i,k+1}\mathcal{T}_{k+1,i}(\hat{x}_{i})$
	$\displaystyle\hskip 10.0pt-\Phi_{i+1,k+1}^{\rm T}C_{i}^{\rm T}\Gamma_{i}^{-1}(%y_{i}+C_{i}\Phi_{i+1,k+1}\mathcal{B}_{k+1,i+1}\mathcal{U}_{k+1,i+1})\Big{]},$
	$\displaystyle b_{k,2}\triangleq-\Phi_{0,k+1}^{\rm T}P_{0}^{-1}\Phi_{0,k+1}%\mathcal{T}_{k+1,0}(\hat{x}_{0})$
	$\displaystyle b_{k,3}\triangleq A_{k}^{-{\rm T}}F_{k}A_{k}^{-1}(A_{k}\hat{x}_{%k}+B_{k}u_{k})-C_{k}^{\rm T}\Gamma_{k}^{-1}y_{k}.$

Note that $b_{k,1}$ is the sum of first $k-1$ terms of summation in (59), $b_{k,3}$ is the last term of summation in (59), and $b_{k,2}$ is the remaining term outside the summation. Next, note, for all $0\leq i\leq k$ , the identities

	$\displaystyle\Phi_{i,k+1}\mathcal{T}_{k+1,i}(\hat{x}_{i})=\Phi_{i,k}\mathcal{T%}_{k,i}(\hat{x}_{i})+\Phi_{i,k+1}B_{k}u_{k},$		(61)
	$\displaystyle\Phi_{i+1,k+1}\mathcal{B}_{k+1,i+1}\mathcal{U}_{k+1,i+1}$
	$\displaystyle\hskip 30.0pt=\Phi_{i+1,k}\mathcal{B}_{k,i+1}\mathcal{U}_{k,i+1}+%\Phi_{i+1,k+1}B_{k}u_{k}.$		(62)

Using (61) and (62), we can write $b_{k,1}=b_{k,1,1}+b_{k,1,2}$ , where

	$\displaystyle b_{k,1,1}$	$\displaystyle\triangleq\sum_{i=0}^{k-1}\big{[}\Phi_{i,k+1}^{\rm T}F_{i}\Phi_{i%,k}\mathcal{T}_{k,i}(\hat{x}_{i})$
		$\displaystyle\hskip 20.0pt-\Phi_{i+1,k+1}^{\rm T}C_{i}^{\rm T}\Gamma_{i}^{-1}(%y_{i}+C_{i}\Phi_{i+1,k}\mathcal{B}_{k,i+1}\mathcal{U}_{k,i+1})\big{]},$
	$\displaystyle b_{k,1,2}$	$\displaystyle\triangleq\sum_{i=0}^{k-1}\big{[}\Phi_{i,k+1}^{\rm T}F_{i}\Phi_{i%,k+1}B_{k}u_{k}$
		$\displaystyle\hskip 20.0pt-\Phi_{i+1,k+1}^{\rm T}C_{i}^{\rm T}\Gamma_{i}^{-1}C%_{i}\Phi_{i+1,k+1}B_{k}u_{k}\big{]}.$

Similarly, (61) implies that $b_{k,2}=b_{k,2,1}+b_{k,2,2}$ , where

	$\displaystyle b_{k,2,1}\triangleq-\Phi_{0,k+1}^{\rm T}P_{0}^{-1}\Phi_{0,k}%\mathcal{T}_{k,0}(\hat{x}_{0}),$
	$\displaystyle b_{k,2,2}\triangleq-\Phi_{0,k+1}^{\rm T}P_{0}^{-1}\Phi_{0,k+1}B_%{k}u_{k}.$

It then follows from (59) and (58), respectively, that

	$\displaystyle b_{k,1,1}+b_{k,2,1}$	$\displaystyle=A_{k}^{-{\rm T}}b_{k-1},$
	$\displaystyle b_{k,1,2}+b_{k,2,2}$	$\displaystyle=-A_{k}^{-{\rm T}}H_{k-1}A_{k}^{-1}B_{k}u_{k}.$

Hence, we obtain the recursive update

	$\displaystyle b_{k}=$	$\displaystyle A_{k}^{-{\rm T}}b_{k-1}-A_{k}^{-{\rm T}}H_{k-1}A_{k}^{-1}B_{k}u_%{k}$
		$\displaystyle+A_{k}^{-{\rm T}}F_{k}A_{k}^{-1}(A_{k}\hat{x}_{k}+B_{k}u_{k})-C_{%k}^{\rm T}\Gamma_{k}^{-1}y_{k}.$

Finally, note that (58) can be used to rewrite (18) as $F_{k}\prec H_{k-1}$ , and hence $H_{k-1}-F_{k}\succ 0$ .Furthermore, since $\Gamma_{k}$ is positive definite, it follows from (60) that $H_{k}$ is positive definite.Therefore, by Lemma 2, the unique minimizer $\hat{x}_{k+1}$ of $J_{k}$ is given by $\hat{x}_{k+1}=-H_{k}^{-1}b_{k}$ , which simplifies to

	$\displaystyle\hat{x}_{k+1}=-H_{k}^{-1}b_{k}=-P_{k+1}b_{k}$
	$\displaystyle=P_{k+1}\big{[}-A_{k}^{-{\rm T}}b_{k-1}+A_{k}^{-{\rm T}}H_{k-1}A_%{k}^{-1}B_{k}u_{k}$
	$\displaystyle\hskip 40.0pt-A_{k}^{-{\rm T}}F_{k}A_{k}^{-1}(A_{k}\hat{x}_{k}+B_%{k}u_{k})+C_{k}^{\rm T}\Gamma_{k}^{-1}y_{k}\big{]}$
	$\displaystyle=P_{k+1}\big{[}A_{k}^{-{\rm T}}P_{k}^{-1}\hat{x}_{k}+A_{k}^{-{\rmT%}}P_{k}^{-1}A_{k}^{-1}B_{k}u_{k}$
	$\displaystyle\hskip 40.0pt-A_{k}^{-{\rm T}}F_{k}A_{k}^{-1}(A_{k}\hat{x}_{k}+B_%{k}u_{k})+C_{k}^{\rm T}\Gamma_{k}^{-1}y_{k}\big{]}$
	$\displaystyle=P_{k+1}\big{[}A_{k}^{-{\rm T}}(P_{k}^{-1}-F_{k})A_{k}^{-1}(A_{k}%\hat{x}_{k}+B_{k}u_{k})+C_{k}^{\rm T}\Gamma_{k}^{-1}y_{k}\big{]}$
	$\displaystyle=P_{k+1}\big{[}C_{k}^{\rm T}\Gamma_{k}^{-1}y_{k}-C_{k}^{\rm T}%\Gamma_{k}^{-1}C_{k}(A_{k}\hat{x}_{k}+B_{k}u_{k})$
	$\displaystyle\hskip 10.0pt+\left(A_{k}^{-{\rm T}}(P_{k}^{-1}-F_{k})A_{k}^{-1}+%C_{k}^{\rm T}\Gamma_{k}^{-1}C_{k}\right)(A_{k}\hat{x}_{k}+B_{k}u_{k})\big{]}$
	$\displaystyle=P_{k+1}\big{[}C_{k}^{\rm T}\Gamma_{k}^{-1}(y_{k}-C_{k}(A_{k}\hat%{x}_{k}+B_{k}u_{k}))$
	$\displaystyle\hskip 40.0pt+P_{k+1}^{-1}(A_{k}\hat{x}_{k}+B_{k}u_{k})\big{]}$
	$\displaystyle=A_{k}\hat{x}_{k}+B_{k}u_{k}+P_{k+1}C_{k}^{\rm T}\Gamma_{k}^{-1}(%y_{k}-C_{k}(A_{k}\hat{x}_{k}+B_{k}u_{k})).$

Hence, (24) is satisfied. Finally, since $H_{k}$ is positive definite, it follows that $P_{k+1}=H_{k}^{-1}$ is also positive definite.∎

$\displaystyle P_{k+1}^{-1}=$	$\displaystyle A_{k}^{-{\rm T}}(P_{k}^{-1}-F_{k})A_{k}^{-1}+C_{k}^{\rm T}\Gamma%_{k}^{-1}C_{k},$	(23)
$\displaystyle\hat{x}_{k+1}=$	$\displaystyle A_{k}\hat{x}_{k}+B_{k}u_{k}$
	$\displaystyle+P_{k+1}C_{k}^{\rm T}\Gamma_{k}^{-1}\left(y_{k}-C_{k}(A_{k}\hat{x%}_{k}+B_{k}u_{k})\right),$	(24)