![]() |
Previous | Next |
[
xOut,
uOut,
info] = ckbs_affine(
max_itr,
epsilon,
z,
b,
g,
h,
db,
dg,
dh,
qinv,
rinv)
\[
\begin{array}{rcl}
S ( x_1 , \ldots , x_N ) & = & \sum_{k=1}^N S_k ( x_k , x_{k-1} ) \\
S_k ( x_k , x_{k-1} ) & = &
\frac{1}{2}
( z_k - h_k - H_k * x_k )^\R{T} * R_k^{-1} * ( z_k - h_k - H_k * x_k )
\\
& + &
\frac{1}{2}
( x_k - g_k - G_k * x_{k-1} )^\R{T} * Q_k^{-1} * ( x_k - g_k - G_k * x_{k-1} )
\\
\end{array}
\]
where the matrices
R_k
and
Q_k
are
symmetric positive definite and
x_0
is the constant zero.
Note that
g_1
is the initial state estimate
and
Q_1
is the corresponding covariance.
\[
\begin{array}{rll}
{\rm minimize}
& S( x_1 , \ldots , x_N )
& {\rm w.r.t.} \; x_1 \in \B{R}^n , \ldots , x_N \in \B{R}^n
\\
{\rm subject \; to}
& b_k + B_k * x_k \leq 0
& {\rm for} \; k = 1 , \ldots , N
\end{array}
\]
( x_1 , \ldots , x_N )
is considered a solution
if there is a Lagrange multiplier sequence
( u_1 , \ldots , u_N )
such that the following conditions are satisfied.
\[
\begin{array}{rcl}
b_k + B_k * x_k & \leq & \varepsilon \\
0 & \leq & u_k \\
| ( B_k^T * u_k + d_k )_j | & \leq & \varepsilon \\
| (u_k)_i * ( b_k + B_k * x_k)_i | & \leq & \varepsilon
\end{array}
\]
for
j = 1 , \ldots , n
,
i = 1 , \ldots , \ell
, and
k = 1 , \ldots , N
.
Here
d_k
is the partial derivative of
S ( x_1 , \ldots , x_N )
with respect to
x_k
and
(u_k)_i
denotes the i-th component of
u_k
.
\[
\varepsilon = epsilon
\]
k = 1 , \ldots , N
\[
z_k = z(:, k)
\]
and z has size
m \times N
.
k = 1 , \ldots , N
\[
b_k = b(:, k)
\]
and b has size
\ell \times N
.
If
\ell = 0
, the problem is not constrained; i.e.,
it is the affine Kalman-Bucy smoother problem.
k = 1 , \ldots , N
\[
g_k = g(:, k)
\]
and g has size
n \times N
.
The value
g_1
serves as the initial state estimate.
k = 1 , \ldots , N
\[
h_k = h(:, k)
\]
and h has size
m \times N
.
k = 1 , \ldots , N
\[
B_k = db(:,:,k)
\]
and db has size
\ell \times n \times N
.
k = 1 , \ldots , N
\[
G_k = dg(:,:,k)
\]
and dg has size
n \times n \times N
.
The initial state estimate
g_1
does not depend on the value of
x_0
, hence
G_1
should be zero.
k = 1 , \ldots , N
\[
H_k = dh(:,:,k)
\]
and dh has size
m \times n \times N
.
k = 1 , \ldots , N
\[
Q_k^{-1} = qinv(:,:, k)
\]
and qinv has size
n \times n \times N
.
The value of
Q_k
is the variance of the initial state
estimate
g_1
.
k = 1 , \ldots , N
\[
R_k^{-1} = rinv(:,:, k)
\]
and rinv has size
m \times m \times N
.
It is ok to signify a missing data value by setting the corresponding
row and column of rinv to zero. This also enables use
to interpolate the state vector
x_k
to points where
there are no measurements.
( x_1 , \ldots , x_N )
.
For
k = 1 , \ldots , N
\[
x_k = xOut(:, k)
\]
and xOut is a two dimensional array with size
n \times N
.
k = 1 , \ldots , N
\[
u_k = uOut(:, k)
\]
and uOut is a two dimensional array with size
\ell \times N
.
The pair xOut, uOut satisfy the
first order conditions
.
ckbs_affine
has satisfied the convergence condition if
and only if
all(
info(end, 1:3) <=
epsilon )
We use
x_k^q
(
u_k^q
)
to denote the state vector (dual vector)
for time point
k
and at the end of iteration
q-1
.
We use
d_k^q
for the partial derivative of
S ( x_1^q , \ldots , x_N^q )
with respect to
x_k
.
info(
q, 1)
is
a bound on the constraint functions
at the end of iteration
q-1
. To be specific
\[
info(q, 1) = \max_{i,k} ( b_k + B_k * x_k )_i
\]
info(
q, 2)
is
a bound on the partial derivative of the Lagrangian with respect to
the state vector sequence
at the end of iteration
q-1
:
\[
info(q, 2) = \max \left[ | ( B_k^T * u_k + d_k )_j | \right]
\]
where the maximum is with respect to
j = 1 , \ldots , n
and
k = 1 , \ldots , N
.
info(
q, 3)
is
a bound on the complementarity conditions
at the end of iteration
q-1
:
\[
info(q, 3) = \max \left[ | (u_k)_i * ( b_k + B_k * x_k)_i | \right]
\]
where the maximum is with respect to
i = 1 , \ldots , \ell
and
k = 1 , \ldots , N
.
info(
q, 4)
is the line search step size used during
iteration
q-1
.
Small step sizes indicate problems with the interior point method
used to solve the affine problem
(with the exception that
info(1, 4)
is always zero).
ckbs_affine
.
It returns true if ckbs_affine
passes the test
and false otherwise.