int Unscented_Kalman_Filter::_init(void)
{
      Gaussian_Nonlinear_Model *m=dynamic_cast<Gaussian_Nonlinear_Model *>(model);
      dcovector x0;
      dsymatrix r0;

      m->Get_Init_Parameters(x0,r0);
      M=x0;
      R=r0;

      if(sqrtm(m->Qw,sqrt_Qw))
            return 1;
      if(sqrtm(m->Qv,sqrt_Qv))
            return 1;
      int Nx=  M.l;
      int Nw= sqrt_Qw.n;
      int N= Nx+Nw;
      int i;
      lambda = 3. - N;

      w_0  = lambda / (N + lambda);
      w_0c = lambda / (N + lambda);// + (3. - alpha * alpha )
      w    = 1. /(2.*(N+ lambda));

      sX.resize(2*N+1);
      sW.resize(2*N+1);
      sY.resize(2*N+1);
      Likelihood = 0.;
      return 0;
}
Beispiel #2
0
inline mxArray* do_sqrtm2x2(const mxArray *mxA)
{
    int n = 0;
    
    if (test_size(mxA, 2, 2, n))
    {        
        const Mat2x2<T>* A = (const Mat2x2<T>*)mxGetData(mxA);
        mxArray *mxR = create_cube<T>(2, 2, n);
        Mat2x2<T>* R = (Mat2x2<T>*)mxGetData(mxR);
        
        for (int i = 0; i < n; ++i)
        {
            sqrtm(A[i], R[i]);
        }
        
        return mxR;
    }
    else if (test_size(mxA, 4, 1, n))
    {        
        const Mat2x2<T>* A = (const Mat2x2<T>*)mxGetData(mxA);
        mxArray *mxR = create_mat<T>(4, n);
        Mat2x2<T>* R = (Mat2x2<T>*)mxGetData(mxR);
        
        for (int i = 0; i < n; ++i)
        {
            sqrtm(A[i], R[i]);
        }
        
        return mxR;
    }
    else if (test_size(mxA, 3, 1, n))
    {
        const SMat2x2<T>* A = (const SMat2x2<T>*)mxGetData(mxA);
        mxArray *mxR = create_mat<T>(3, n);
        SMat2x2<T>* R = (SMat2x2<T>*)mxGetData(mxR);
        
        for (int i = 0; i < n; ++i)
        {
            sqrtm(A[i], R[i]);
        }
        
        return mxR;
    }
    else
    {
        mexErrMsgIdAndTxt("sqrtm2x2:invalidarg", "The size of input array is invalid.");
    }
    
    return NULL;
}
int Unscented_Kalman_Filter::SP_Init(void)
{
      int Nx=  M.l;
      int Nw= sqrt_Qw.n;
      int N= Nx+Nw;
      int i;
      dgematrix  sqrt_R;
      double phi = sqrt(N+lambda);

      if(sqrtm(R,sqrt_R))
            {
                  return 1;
            }
      sX[0]=M;
      sW[0].zero();

      for(i=1;i<1+Nx;i++){
	    sX[i]=M + phi*get_column(sqrt_R,i-1);
	    sW[i].zero();
      }
      for(i=Nx+1;i<1+Nx+Nw;i++){
	    sX[i]=M;
	    sW[i]=phi*get_column(sqrt_Qw,i-(Nx+1));
      }
      for(i=1+Nx+Nw;i<1+2*Nx+Nw;i++){
	    sX[i]=M-phi*get_column(sqrt_R,i-(1+Nx+Nw));
	    sW[i].zero();
      }
      for(i=1+2*Nx+Nw;i<1+2*Nx+2*Nw;i++){
	    sX[i]=M;
	    sW[i]=-phi*get_column(sqrt_Qw,i-(1+2*Nx+Nw));
      }
      return 0;
}
Beispiel #4
0
// Square root of a square matrix (based on Octave sqrtm implementation)
cmat sqrtm(const mat& A)
{
    return sqrtm(to_cmat(A));
}