kernelforwardbackwardsmoother.hpp

dysii is a C++ library for distributed probabilistic inference and learning in large-scale dynamical

     * together */
    {
      assert (p_xtnp1_ytn.getSize() == p_xtnp1_ytT.getSize());

      aux::KDTree<ST> queryTree(&q_xtnp1s);
      aux::KDTree<ST> targetTree(&p_xtnp1_ytT);

      aux::matrix ws(2,p_xtnp1_ytn.getSize());
      row(ws,0) = p_xtnp1_ytT.getWeights();
      row(ws,1) = p_xtnp1_ytn.getWeights();
      
      aux::matrix result(2,q_xtnp1s.getSize());
      noalias(result) = aux::distributedDualTreeDensity(queryTree,
          targetTree, ws, N, K);
          
      noalias(psi) = row(result,0);
      noalias(delta) = row(result,1);
    }
  } else {
    /* filter density evaluation */
    {
      aux::vector mu(D);
      aux::lower_triangular_matrix L(D,D);
      noalias(mu) = p_xtn_ytn.getDistributedExpectation();
      noalias(L) = p_xtn_ytn.getDistributedStandardDeviation();
  
      DiracMixturePdf q(q_xtns);
      q.standardise(mu, L);
      p_xtn_ytn.standardise(mu, L);

      //p_xtn_ytn.redistributeBySpace();
      aux::KDTree<ST> queryTree(&q);
      aux::KDTree<ST> targetTree(&p_xtn_ytn);    
      noalias(pi) = aux::distributedDualTreeDensity(queryTree,
          targetTree, p_xtn_ytn.getWeights(), N, K);
    }
    
    /* smooth density evaluation */
    {
      aux::vector mu(D);
      aux::lower_triangular_matrix L(D,D);
      noalias(mu) = p_xtnp1_ytT.getDistributedExpectation();
      noalias(L) = p_xtnp1_ytT.getDistributedStandardDeviation();
  
      DiracMixturePdf q(q_xtnp1s);
      q.standardise(mu, L);
      p_xtnp1_ytT.standardise(mu, L);

      //p_xtnp1_ytT.redistributeBySpace();
      aux::KDTree<ST> queryTree(&q);
      aux::KDTree<ST> targetTree(&p_xtnp1_ytT);
      noalias(psi) = aux::distributedDualTreeDensity(queryTree,
          targetTree, p_xtnp1_ytT.getWeights(), N, K);
    }
    
    /* uncorrected filter density evaluation */
    {
      aux::vector mu(D);
      aux::lower_triangular_matrix L(D,D);
      noalias(mu) = p_xtnp1_ytn.getDistributedExpectation();
      noalias(L) = p_xtnp1_ytn.getDistributedStandardDeviation();
  
      DiracMixturePdf q(q_xtnp1s);
      q.standardise(mu, L);
      p_xtnp1_ytn.standardise(mu, L);

      //p_xtnp1_ytn.redistributeBySpace();
      aux::KDTree<ST> queryTree(&q);
      aux::KDTree<ST> targetTree(&p_xtnp1_ytn);
      noalias(delta) = aux::distributedDualTreeDensity(queryTree,
          targetTree, p_xtnp1_ytn.getWeights(), N, K);
    }
  }

  /* build smoothed distribution */
  psi = element_div(element_prod(pi,psi), element_prod(delta,q_ptns));
  this->p_xtn_ytT = q_xtns;
  this->p_xtn_ytT.setWeights(psi);
  
  /* for at least one system, double well, normalisation has proved
   * necessary to prevent degeneracy, and so we include it here. */
  this->p_xtn_ytT.distributedNormalise();
  
  /* update state */
  this->tn = tn;
}

template <class T, class NT, class KT, class ST>
void indii::ml::filter::KernelForwardBackwardSmoother<T,NT,KT,ST>::smooth(
    const T tn,
    indii::ml::aux::DiracMixturePdf& p_xtn_ytn,
    indii::ml::aux::DiracMixturePdf& p_xtnp1_ytn,
    const unsigned int flags) {
  const unsigned int D = model->getStateSize();
    
  /* pre-conditions */
  assert (p_xtn_ytn.getDimensions() == D);
  assert (p_xtnp1_ytn.getDimensions() == D);

  vector x(D);
  const T del = this->tn - tn;
  DiracMixturePdf& p_xtnp1_ytT = this->p_xtn_ytT;
  unsigned int P_local = p_xtn_ytn.getSize();
  unsigned int i;

  /* pick apart flags */
  bool noResampling = flags & NO_RESAMPLING;
  bool noStandardisation = flags & NO_STANDARDISATION;
  bool filterSmoother = flags & FILTER_SMOOTHER;

  q_xtns = p_xtn_ytn;
  if (noResampling && noStandardisation && filterSmoother) {
    /* use filter-smoother */
    q_xtnp1s = p_xtnp1_ytT;
    assert (p_xtn_ytn.getSize() == p_xtnp1_ytT.getSize());  

    aux::vector psi(p_xtnp1_ytT.getWeights());
    this->p_xtn_ytT = p_xtn_ytn;
    this->p_xtn_ytT.setWeights(psi);

    /* update state */
    this->tn = tn;

    return;
  } else if (noResampling) {
    /* use p_xtnp1_ytn particles as propagations */
    q_xtnp1s = p_xtnp1_ytT;
    assert (p_xtn_ytn.getSize() == p_xtnp1_ytT.getSize());  
  } else {
    /* propagate filter particles */
    this->q_delta = del;
    q_xtnp1s.clear();
    for (i = 0; i < p_xtn_ytn.getSize(); i++) {
      noalias(x) = model->transition(p_xtn_ytn.get(i), tn, del);
      q_xtnp1s.add(x, p_xtn_ytn.getWeight(i));
    }
  }

  const aux::vector& pi = p_xtn_ytn.getWeights();
  aux::vector delta(P_local), psi(P_local);
  
  if (noStandardisation) {
    /* uncorrected and smooth densities have same support, evaluate
     * together */
    assert (p_xtnp1_ytn.getSize() == p_xtnp1_ytT.getSize());
    
    if (noResampling) {
      /* query tree also has same support, perform self-tree evaluation */
      aux::KDTree<ST> tree(&p_xtnp1_ytT);

      aux::matrix ws(2,p_xtnp1_ytn.getSize());
      row(ws,0) = p_xtnp1_ytT.getWeights();
      row(ws,1) = p_xtnp1_ytn.getWeights();

      aux::matrix result(2,p_xtnp1_ytn.getSize());
      noalias(result) = aux::distributedSelfTreeDensity(tree, ws, N, K,
          false);

      noalias(psi) = row(result,0);
      noalias(delta) = row(result,1);
    } else {
      /* uncorrected and smooth densities have same support, evaluate
       * together */
      assert (p_xtnp1_ytn.getSize() == p_xtnp1_ytT.getSize());

      //p_xtnp1_ytT.redistributeBySpace();
      //p_xtnp1_ytn.redistributeBySpace();
      aux::KDTree<ST> queryTree(&q_xtnp1s);
      aux::KDTree<ST> targetTree(&p_xtnp1_ytT);

      aux::matrix ws(2,p_xtnp1_ytn.getSize());
      row(ws,0) = p_xtnp1_ytT.getWeights();
      row(ws,1) = p_xtnp1_ytn.getWeights();
      
      aux::matrix result(2,q_xtnp1s.getSize());
      noalias(result) = aux::distributedDualTreeDensity(queryTree,
          targetTree, ws, N, K);

      noalias(psi) = row(result,0);
      noalias(delta) = row(result,1);
    }
  } else {
    /* smooth density evaluation */
    {
      aux::vector mu(D);
      aux::lower_triangular_matrix L(D,D);
      noalias(mu) = p_xtnp1_ytT.getDistributedExpectation();
      noalias(L) = p_xtnp1_ytT.getDistributedStandardDeviation();
      p_xtnp1_ytT.standardise(mu, L);
      //p_xtnp1_ytT.redistributeBySpace();

      if (noResampling) {
        /* query tree has same support, perform self-tree evaluation */
        aux::KDTree<ST> tree(&p_xtnp1_ytT);
        noalias(delta) = aux::distributedSelfTreeDensity(tree,
            p_xtnp1_ytT.getWeights(), N, K);
      } else {
        DiracMixturePdf q(q_xtnp1s);
        q.standardise(mu, L);

        aux::KDTree<ST> queryTree(&q);
        aux::KDTree<ST> targetTree(&p_xtnp1_ytT);
        noalias(delta) = aux::distributedDualTreeDensity(queryTree,
            targetTree, p_xtnp1_ytT.getWeights(), N, K);
      }
    }
  
    /* uncorrected filter density evaluation */
    {
      aux::vector mu(D);
      aux::lower_triangular_matrix L(D,D);
      noalias(mu) = p_xtnp1_ytn.getDistributedExpectation();
      noalias(L) = p_xtnp1_ytn.getDistributedStandardDeviation();
      p_xtnp1_ytn.standardise(mu, L);
      //p_xtnp1_ytn.redistributeBySpace();
  
      if (noResampling) {
        /* query tree has same support, perform self-tree evaluation */
        aux::KDTree<ST> tree(&p_xtnp1_ytn);
        noalias(delta) = aux::distributedSelfTreeDensity(tree,
            p_xtnp1_ytn.getWeights(), N, K);
      } else {
        DiracMixturePdf q(q_xtnp1s);
        q.standardise(mu, L);

        aux::KDTree<ST> queryTree(&q);
        aux::KDTree<ST> targetTree(&p_xtnp1_ytn);
        noalias(delta) = aux::distributedDualTreeDensity(queryTree,
            targetTree, p_xtnp1_ytn.getWeights(), N, K);
      }
    }
  }
  
  /* build smoothed distribution */
  psi = element_div(element_prod(pi,psi), delta);
  this->p_xtn_ytT = p_xtn_ytn;
  this->p_xtn_ytT.setWeights(psi);

  /* for at least one system, double well, normalisation has proved
   * necessary to prevent degeneracy, and so we include it here. */
  this->p_xtn_ytT.distributedNormalise();
  
  /* update state */
  this->tn = tn;
}

template <class T, class NT, class KT, class ST>
inline indii::ml::aux::DiracMixturePdf&
    indii::ml::filter::KernelForwardBackwardSmoother<T,NT,KT,ST>::getProposals() {
  return q_xtns;
}

template <class T, class NT, class KT, class ST>
inline indii::ml::aux::DiracMixturePdf&
    indii::ml::filter::KernelForwardBackwardSmoother<T,NT,KT,ST>::getPropagations() {
  return q_xtnp1s;
}

template <class T, class NT, class KT, class ST>
indii::ml::aux::DiracMixturePdf
    indii::ml::filter::KernelForwardBackwardSmoother<T,NT,KT,ST>::smoothedMeasure() {
  namespace aux = indii::ml::aux;
    
  unsigned int i;
  StratifiedParticleResampler resampler;
  aux::DiracMixturePdf resampled(resampler.resample(
      this->getSmoothedState()));
  indii::ml::aux::DiracMixturePdf p_ytn_xtT(model->getMeasurementSize());
  
  for (i = 0; i < resampled.getSize(); i++) {
    p_ytn_xtT.add(model->measure(resampled.get(i)));
  }

  return p_ytn_xtT;
}

template <class T, class NT, class KT, class ST>
void indii::ml::filter::KernelForwardBackwardSmoother<T,NT,KT,ST>::smoothedResample(
    ParticleResampler* resampler) {
  indii::ml::aux::DiracMixturePdf resampled(resampler->resample(
      this->getSmoothedState()));
  this->setSmoothedState(resampled);
}

#endif