|
|
Previous | Next |
[dPPlus, dPMinus, dR, dS, dY] = ckbs_newton_step_L1(
mu, s, y, r, b, d, Bdia, Hdia, Hlow,
pPlus, pMinus)
\mu
-relaxed affine robust L1 Kalman-Bucy smoother problem.
\mu \in \B{R}_+
,
s \in \B{R}^{m \times N}
,
y \in \B{R}^{n \times N}
,
r \in \B{R}^{m \times N}
,
b \in \B{R}^{m \times N}
,
d \in \B{R}^{n \times N}
,
B \in \B{R}^{m \times n \times N}
,
H \in \B{R}^{n \times n \times N}
,
p^+ \in \B{R}^{m \times N}
,
p^- \in \B{R}^{m \times N}
,
the
\mu
-relaxed affine L1 robust Kalman-Bucy smoother problem is:
\[
\begin{array}{ll}
{\rm minimize}
& \frac{1}{2} y^\R{T} H(x) y + d(x)^\R{T} y
+ \sqrt{\B{2}}^\R{T} (p^+ + p^-) -\mu \sum_{i =
1}^{mN} \log(p_i^+) - \sum_{i=1}^{mN} \mu \log(p_i^-)
\;{\rm w.r.t} \; y \in \B{R}^{nN}\; , \; p^+ \in \B{R}_+^{M} \; , \; p^- \in \B{R}_+^{M}
\\
{\rm subject \; to} & b(x) + B(x) y - p^+ + p^- = 0
\end{array}
\]
In addition,
H
is symmetric block tri-diagonal with each block of
size
n \times n
and
B
is block diagonal with each block of size
m \times n
r, \; s \in \B{R}^{m \times N}
to denote
\mu /p^+\;,\; \mu/p^-
, respectively, and
we denote by
q
the lagrange multiplier associated to the
equality constraint. We also use
\B{1}
to denote the vector of length
mN
with all its components
equal to one, and
\B{\sqrt{2}}
to denote the vector of
length
mN
with all its components equal to
\sqrt{2}
.
The corresponding Lagrangian is
\[
L(p^+, p^-, y, q) =
\frac{1}{2} y^\R{T} H y + d^\R{T} y + \B{\sqrt{2}}^T(p^+ + p^-)
- \mu \sum_{i=1}^{mN} \log(p_i^+) - \mu\sum_{i=1}^{mN}\log(p_i^-)
+ q^\R{T} (b + B y - p^+ + p^-)
\]
The partial gradients of the Lagrangian are given by
\[
\begin{array}{rcl}
\nabla_p^+ L(p^+, p^-, y, q ) & = & \B{\sqrt{2}} - q - r \\
\nabla_p^- L(p^+, p^-, y, q) & = & \B{\sqrt{2}} + q - s \\
\nabla_y L(p^+, p^-, y, q ) & = & H y + c + B^\R{T} q \\
\nabla_q L(p^+, p^-, y, q ) & = & b + B y - p^+ + p^- \\
\end{array}
\]
From the first two of the above equations,
we have
q = (r - s)/2
.
D(s)
to denote the diagonal matrix with
s
along its diagonal.
The Kuhn-Tucker Residual function
F : \B{R}^{4mN + nN} \rightarrow \B{R}^{4mN + nN}
is defined by
\[
F(p^+, p^-, r, s, y)
=
\left(
\begin{array}{c}
p^+ - p^- - b - B y \\
D(p^-) D(s) \B{1} - \tau \B{1} \\
r + s - 2 \B{\sqrt{2}} \\
D(p^+) D(r ) \B{1} - \tau \B{1} \\
H y + d + B^\R{T} (r - s)/2
\end{array}
\right)
\]
The Kuhn-Tucker conditions for a solution of the
\mu
-relaxed constrained affine Kalman-Bucy smoother problem is
F(p^+, p^-, r, s, y) = 0
.
(p^+, p^-, r, s, y)
, the Newton step
( (\Delta p^+)^\R{T} , (\Delta p^-)^\R{T} , \Delta
r^\R{T}, \Delta s^\R{T}, \Delta y^\R{T} )^\R{T}
solves the problem:
\[
F_\mu^{(1)} (p^+, p^-, r, s, y)
\left( \begin{array}{c} \Delta p^+ \\ \Delta p^- \\ \Delta r \\
\Delta s \\ \Delta y \end{array} \right)
=
- F(p^+, p^-, r, s, y)
\]
mu
is a positive scalar specifying the
relaxation parameter
\mu
.
s
is an array of size
m \times
N
.
All the elements of
s
are greater than zero.
y
is an array of size
n \times
N
r
is an array of size
m \times
N
. All the elements of
r
are greater than zero.
b
is an array of size
m \times N
.
d
is an array of size
n \times
N
.
Bdia
is an
m \times n \times N
array.
For
k = 1 , \ldots , N
we define
B_k \in \B{R}^{m \times n}
by
\[
B_k = Bdia(:, :, k)
\]
B
is defined by
\[
B
=
\left( \begin{array}{cccc}
B_1 & 0 & 0 & \\
0 & B_2 & 0 & 0 \\
0 & 0 & \ddots & 0 \\
& 0 & 0 & B_N
\end{array} \right)
\]
Hdia
is an
n \times n \times N
array.
For
k = 1 , \ldots , N
we define
H_k \in \B{R}^{n \times n}
by
\[
H_k = Hdia(:, :, k)
\]
Hlow
is an
n \times n \times N
array.
For
k = 1 , \ldots , N
we define
L_k \in \B{R}^{n \times n}
by
\[
L_k = Hlow(:, :, k)
\]
H
is defined by
\[
H
=
\left( \begin{array}{cccc}
H_1 & L_2^\R{T} & 0 & \\
L_2 & H_2 & L_3^\R{T} & 0 \\
0 & \ddots & \ddots & \ddots \\
& 0 & L_N & H_N
\end{array} \right)
\]
dPPlus
is an array of size
m \times N
equal to the
\Delta p^+
components of the Newton step.
dPMinus
is an array of size
m \times N
equal to the
\Delta p^-
components of the Newton step.
dR
is an array of size
m \times N
equal to the
\Delta r
components of the Newton step.
dS
is an array of size
m \times N
equal to the
\Delta s
components of the Newton step.
dY
is an array of size
n \times N
equal to the
\Delta y
components of the Newton step.
ckbs_newton_step_L1.
It returns true if ckbs_newton_step_L1 passes the test
and false otherwise.
F
is given by
\[
F_\mu^{(1)} (p^+, p^-, r, s, y) =
\left(
\begin{array}{ccccccc}
I & -I & 0 & 0 & -B \\
0 & D( s^- ) & 0 & D(p^-) & 0 \\
0 & 0 & I & I & 0 \\
D( s^+) & 0 & D( p^+ ) & 0 & 0 \\
0 & 0 & - 0.5 B^\R{T} & 0.5 B^\R{T} & C
\end{array}
\right)
\]
Given the inputs
p^+ , p^-, s^+ , s^- , y , B , b , C
and
c
,
the following algorithm solves the Newton System for
\Delta p^+ , \Delta p^- , \Delta s^+ , \Delta s^-
,
and
\Delta y
:
\[
\begin{array}{cccccc}
\bar{d} &= & \tau \B{1} / s^+ - \tau \B{1} / s^- - b - B y + p^+
\\
\bar{e} &= & B^\R{T} ( \sqrt{\B{2}} - s^- ) - C y - c
\\
\bar{f} &= & \bar{d} - D( s^+ )^{-1} D( p^+ ) ( 2 \sqrt{\B{2}} - s^- )
\\
T &= & D( s^+ )^{-1} D( p^+ ) + D( s^- )^{-1} D( p^- )
\\
\Delta y &= &
[ C + B^\R{T} T^{-1} B ]^{-1} ( \bar{e} + B^\R{T} T^{-1} \bar{f} )
\\
\Delta s^- &= &
T^{-1} B \Delta y - T^{-1} \bar{f}
\\
\Delta s^+ &= &
- \Delta s^- + 2 \sqrt{\B{2}} - s^+ - s^-
\\
\Delta p^- &= &
D( s^- )^{-1} [ \tau \B{1} - D( p^- ) \Delta s^- ] - p^-
\\
\Delta p^+ &= &
\Delta p^- + B \Delta y + b + B y - p^+ + p^-
\end{array}
\]
where the matrix
T
is diagonal with positive elements,
and the matrix
C + B^\R{T} T^{-1} B \in \B{R}^{N n \times N n }
is
block tridiagonal positive definite.