|
|
Previous | Next |
[grad] = ckbs_t_hess(
x, z, g_fun, h_fun, qinv, rinv, params)
\[
\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.
Q_{N+1}
to be the
n \times n
identity
matrix and
G_{N+1}
to be zero,
\[
\begin{array}{rcl}
\nabla_k S_k^{(1)} ( x_k , x_{k-1} )
& = & H_k^\R{T} * R_k^{-1} * ( h_k + H_k * x_k - z_k )
+ Q_k^{-1} * ( x_k - g_k - G_k * x_{k-1} )
\\
\nabla_k S_{k+1}^{(1)} ( x_{k+1} , x_k )
& = & G_{k+1}^\R{T} * Q_{k+1}^{-1} * ( g_{k+1} + G_{k+1} * x_k - x_{k+1} )
\end{array}
\]
It follows that the gradient of the
affine Kalman-Bucy smoother residual sum of squares is
\[
\begin{array}{rcl}
\nabla S ( x_1 , \ldots , x_N )
& = &
\left( \begin{array}{c}
d_1 \\ \vdots \\ d_N
\end{array} \right)
\\
d_k & = & \nabla_k S_k^{(1)} ( x_k , x_{k-1} )
+ \nabla_k S_{k+1}^{(1)} ( x_{k+1} , x_k )
\end{array}
\]
where
S_{N+1} ( x_{N+1} , x_N )
is defined as
identically zero.
x
is a two dimensional array,
for
k = 1 , \ldots , N
\[
x_k = x(:, k)
\]
and
x
has size
n \times N
.
z
is a two dimensional array,
for
k = 1 , \ldots , N
\[
z_k = z(:, k)
\]
and
z
has size
m \times N
.
g_fun
is a function handle for the
process model.
h_fun
is a function handle for the
measurement model.
qinv
is a three dimensional array,
for
k = 1 , \ldots , N
\[
Q_k^{-1} = qinv(:,:,k)
\]
and
qinv
has size
n \times n \times N
.
rinv
is a three dimensional array,
for
k = 1 , \ldots , N
\[
R_k^{-1} = rinv(:,:,k)
\]
and
rinv
has size
m \times m \times N
.
params
is a structure containing the
requisite parameters.
D
is a three dimensional array,
for
k = 1 , \ldots , N
\[
D_k = hess(:, :, k)
\]
and
D
has size
n\times n \times N
.
A
is a three dimensional array,
for
k = 1 , \ldots , N
\[
A_k = hess(:,:,k)
\]
and
A
has size
n\times n \times N
.
ckbs_t_hess.
It returns true if ckbs_t_hess passes the test
and false otherwise.