|
|
Previous | Next |
[x_out, info] = ckbs_t_general(
g_fun, h_fun, ...
max_itr, epsilon, x_in, z, 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} ) & = &
\rho_k^p\left([ z_k - h_k ( x_k ) ]^\R{T} * R_k^{-1} * [ z_k - h_k ( x_k
) ]\right)
\\
& + &
\rho_k^m\left([ x_k - g_k ( x_{k-1} ) ]^\R{T} * Q_k^{-1} * [ x_k - g_k (
x_{k-1} ) ]\right)
\\
\end{array}
\]
where
\rho_k^p
and
\rho_k^m
are either quadratic, Student's t,
or component-wise mixtures of these functions,
depending on user-specified indices. The matrices
R_k
and
Q_k
are
symmetric positive definite and
x_0
is the constant zero.
Note that the process model
g_fun
should provide an initial state estimate for
k=1
,
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
\end{array}
\]
ckbs_nonlinear argument
g_fun
is a function that supports both of
the following syntaxes
[gk] = feval(g_fun, k, xk1)
[gk, Gk] = feval(g_fun, k, xk1)
g_fun
argument
k
is an integer with
1 \leq k \leq N
.
The case
k = 1
serves to specify the initial state estimate.
g_fun
argument
xk1
is an column vector with
length
n
. It specifies the state vector at time index
k
\[
xk1 = x_{k-1}
\]
.
In the case
k = 1
, the value of
xk1
does not matter.
g_fun
result
gk
is an column vector with
length
n
and
\[
gk = g_k ( x_{k-1} )
\]
In the case
k = 1
, the value of
gk
is the initial state
estimate at time index
k
.
g_fun
result
Gk
is present in the syntax,
it is the
n \times n
matrix given by
and
\[
Gk = \partial_{k-1} g_k ( x_{k-1} )
\]
In the case
k = 1
, the value of
Gk
is the zero matrix;
i.e.,
g_k
does not depend on the value of
x_{k-1}
.
ckbs_nonlinear argument
h_fun
is a function that supports both of the
following syntaxes
[hk] = feval(h_fun, k, xk)
[hk, Hk] = feval(h_fun, k, xk)
h_fun
argument
k
is an integer with
1 \leq k \leq N
.
h_fun
argument
xk
is an column vector with
length
n
. It specifies the state vector at time index
k
\[
xk = x_k
\]
.
h_fun
result
hk
is an column vector with
length
m
and
\[
hk = h_k ( x_k )
\]
h_fun
result
Hk
is present in the syntax,
it is the
m \times n
matrix given by
and
\[
Hk = \partial_k h_k ( x_k )
\]
max_itr
specifies the maximum number of
iterations of the algorithm to execute. It must be greater than or
equal to zero. Note that if it is zero, the first row of the
info
return value will still be computed.
This can be useful for deciding what is a good value for the argument
epsilon
.
ckbs_nonlinear argument
epsilon
is a positive scalar.
It specifies the convergence
criteria value; i.e.,
\[
\varepsilon = epsilon
\]
ckbs_nonlinear argument
x_in
is a two dimensional array with size
n \times N
.
It specifies a sequence of state values; i.e.,
for
k = 1 , \ldots , N
\[
x\_in (:, k) = x_k
\]
The closer the initial state sequence is to the solution
the faster, and more likely, the ckbs_nonlinear will converge.
The initial state sequence need not be feasible; i.e.
it is not necessary that
\[
f_k ( x_k ) \leq 0
\]
for all
k
.
ckbs_nonlinear argument
z
is a two dimensional array
with size
m \times N
.
It specifies the sequence of measurement vectors; i.e.,
for
k = 1 , \ldots , N
\[
z(:, k) = z_k
\]
ckbs_nonlinear argument
qinv
is a three dimensional array
with size
n \times n \times N
.
It specifies the inverse of the variance of the measurement noise; i.e.,
for
k = 1 , \ldots , N
\[
qinv(:,:, k) = Q_k^{-1}
\]
In the case
k = 1
, the value of
Q_k
is the variance
of the initial state estimate (see g_fun
.
ckbs_nonlinear argument
rinv
is a three dimensional array,
with size
m \times m \times N
.
it specifies the inverse of the variance of the transition noise; i.e.,
for
k = 1 , \ldots , N
\[
rinv(:,:, k) = R_k^{-1}
\]
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.
params
structure specifies auxiliary parameters required by
the smoother. The structure is passed to the objective, gradient, and
hessian functions; some parameter values are required for these to work
correctly (see below).
level
argument sets the output level. If
level
is greater than 1, numerical derivatives are computed.
dif_proc
is the Student's t degree of freedom parameter used
for any Student's t process component.
dif_meas
is the Student's t degree of freedom parameter used
for any Student's t measurement component.
inds_proc_st
lists indicies of the state (process residual)
that are to be modeled using Student's t. Setting this structure to empty
[]
means quadratic model is used for entire state.
inds_meas_st
lists indicies of the measurement residual
that are to be modeled using Student's t. Setting this structure to empty
[]
means quadratic model is used for entire measurement.
x_out
is a two dimensional array with size
n \times N
.
( x_1 , \ldots , x_N )
.
It contains an approximation for the optimal sequence; i.e.,
for
k = 1 , \ldots , N
\[
x\_out(:, k) = x_k
\]
info
is a matrix with each row corresponding
to an iteration of the algorithm.
Note that ckbs_nonlinear has satisfied the convergence condition if
and only if
all( info(end, 1) <= epsilon )
or
all( info(end, 3) <= epsilon )
ckbs_t_general.
It returns true if ckbs_t_general passes the test
(meaning algorithm converges to stationary point)
and false otherwise.