void SIR_scheme::copy_resamples (ColMatrix& P, const Importance_resampler::Resamples_t& presamples)
/*
* Update P by selectively copying presamples
* Uses a in-place copying alogorithm
* Algorithm: In-place copying
* First copy the live samples (those resampled) to end of P
* Replicate live sample in-place
*/
{
// reverse_copy_if live
std::size_t si = P.size2(), livei = si;
Importance_resampler::Resamples_t::const_reverse_iterator pri, pri_end = presamples.rend();
for (pri = presamples.rbegin(); pri != pri_end; ++pri) {
--si;
if (*pri > 0) {
--livei;
noalias(FM::column(P,livei)) = FM::column(P,si);
}
}
assert (si == 0);
// Replicate live samples
si = 0;
Importance_resampler::Resamples_t::const_iterator pi, pi_end = presamples.end();
for (pi = presamples.begin(); pi != pi_end; ++pi) {
std::size_t res = *pi;
if (res > 0) {
do {
noalias(FM::column(P,si)) = FM::column(P,livei);
++si; --res;
} while (res > 0);
++livei;
}
}
assert (si == P.size2()); assert (livei == P.size2());
}
void SIR_kalman_scheme::roughen_correlated (ColMatrix& P, Float K)
/*
* Roughening
* Uses a roughening noise based on covariance of P
* This is a more sophisticated algorithm then Ref[1] as it takes
* into account the correlation of P
* K is scaleing factor for roughening noise
* Numerical colapse of P
* Numerically when covariance of P semi definite (or close), X's UdU factorisation
* may be negative.
* Exceptions:
* Bayes_filter_exception due colapse of P
* unchanged: P
*/
{
using namespace std;
// Scale variance by constant and state dimensions
Float VarScale = sqr(K) * pow (Float(P.size2()), Float(-2.)/Float(x_size));
update_statistics(); // Estimate sample mean and covariance
// Decorrelate states
Matrix UD(x_size,x_size);
Float rcond = UdUfactor (UD, X);
rclimit.check_PSD(rcond, "Roughening X not PSD");
// Sampled predict model for roughening
FM::identity (roughen_model.Fx);
// Roughening predict based on scaled variance
UdUseperate (roughen_model.G, roughen_model.q, UD);
roughen_model.q *= VarScale;
roughen_model.init_GqG();
// Predict using roughening model
predict (roughen_model);
}
void SIR_scheme::roughen_minmax (ColMatrix& P, Float K) const
/*
* Roughening
* Uses algorithm from Ref[1] using max-min in each state of P
* K is scaleing factor for roughening noise
* unique_samples is unchanged as roughening is used to postprocess observe resamples
* Numerical colapse of P
* P with very small or zero range result in minimal roughening
* Exceptions:
* none
* unchanged: P
*/
{
using namespace std;
// Scale Sigma by constant and state dimensions
Float SigmaScale = K * pow (Float(P.size2()), -1/Float(x_size));
// Find min and max states in all P, precond P not empty
Vec xmin(x_size); noalias(xmin) = column(P,0);
Vec xmax(x_size); noalias(xmax) = xmin;
ColMatrix::iterator2 pi = P.begin2();
while (pi != P.end2()) // Loop includes 0 to simplify code
{
Vec::iterator mini = xmin.begin();
Vec::iterator maxi = xmax.begin();
#ifdef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
for (ColMatrix::iterator1 xpi = begin(pi, ublas::iterator2_tag()); xpi != end(pi, ublas::iterator2_tag()); ++xpi)
#else
for (ColMatrix::iterator1 xpi = pi.begin(); xpi != pi.end(); ++xpi)
#endif
{
if (*xpi < *mini) *mini = Float(*xpi); // ISSUE mixed type proxy assignment
if (*xpi > *maxi) *maxi = Float(*xpi);
++mini; ++maxi;
}
++pi;
}
// Roughening st.dev max-min
Vec rootq(x_size);
rootq = xmax;
noalias(rootq) -= xmin;
rootq *= SigmaScale;
// Apply roughening predict based on scaled variance
DenseVec n(x_size);
for (pi = P.begin2(); pi != P.end2(); ++pi) {
random.normal (n); // independant zero mean normal
// multiply elements by std dev
for (DenseVec::iterator ni = n.begin(); ni != n.end(); ++ni) {
*ni *= rootq[ni.index()];
}
ColMatrix::Column Pi(P,pi.index2());
noalias(n) += Pi; // add to P
Pi = n;
}
}
Bayes_base::Float Unscented_scheme::observe (Correlated_additive_observe_model& h, const FM::Vec& z)
/* Observation fusion
* Pre : x,X
* Post: x,X is PSD
*/
{
std::size_t z_size = z.size();
ColMatrix zXX (z_size, 2*x_size+1);
Vec zp(z_size);
SymMatrix Xzz(z_size,z_size);
Matrix Xxz(x_size,z_size);
Matrix W(x_size,z_size);
observe_size (z.size()); // Dynamic sizing
// Create Unscented distribution
kappa = observe_Kappa(x_size);
Float x_kappa = Float(x_size) + kappa;
unscented (XX, x, X, x_kappa);
// Predict points of XX using supplied observation model
{
Vec zXXi(z_size), zXX0(z_size);
zXX0 = h.h( column(XX,0) );
column(zXX,0) = zXX0;
for (std::size_t i = 1; i < XX.size2(); ++i) {
zXXi = h.h( column(XX,i) );
// Normalise relative to zXX0
h.normalise (zXXi, zXX0);
column(zXX,i) = zXXi;
}
}
// Mean of predicted distribution: zp
noalias(zp) = column(zXX,0) * kappa;
for (std::size_t i = 1; i < zXX.size2(); ++i) {
noalias(zp) += column(zXX,i) / Float(2); // ISSUE uBlas may not be able to promote integer 2
}
zp /= x_kappa;
// Covariance of observation predict: Xzz
// Subtract mean from each point in zXX
for (std::size_t i = 0; i < XX_size; ++i) {
column(zXX,i).minus_assign (zp);
}
// Center point, premult here by 2 for efficiency
{
ColMatrix::Column zXX0 = column(zXX,0);
noalias(Xzz) = FM::outer_prod(zXX0, zXX0);
Xzz *= 2*kappa;
}
// Remaining Unscented points
for (std::size_t i = 1; i < zXX.size2(); ++i) {
ColMatrix::Column zXXi = column(zXX,i);
noalias(Xzz) += FM::outer_prod(zXXi, zXXi);
}
Xzz /= 2*x_kappa;
// Correlation of state with observation: Xxz
// Center point, premult here by 2 for efficiency
{
noalias(Xxz) = FM::outer_prod(column(XX,0) - x, column(zXX,0));
Xxz *= 2*kappa;
}
// Remaining Unscented points
for (std::size_t i = 1; i < zXX.size2(); ++i) {
noalias(Xxz) += FM::outer_prod(column(XX,i) - x, column(zXX,i));
}
Xxz /= 2* (Float(x_size) + kappa);
// Innovation covariance
S = Xzz;
noalias(S) += h.Z;
// Inverse innovation covariance
Float rcond = UdUinversePD (SI, S);
rclimit.check_PD(rcond, "S not PD in observe");
// Kalman gain
noalias(W) = prod(Xxz,SI);
// Normalised innovation
h.normalise(s = z, zp);
noalias(s) -= zp;
// Filter update
noalias(x) += prod(W,s);
RowMatrix WStemp(W.size1(), S.size2());
noalias(X) -= prod_SPD(W,S, WStemp);
return rcond;
}