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;
}
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;
}
// Square root of a square matrix (based on Octave sqrtm implementation)
cmat sqrtm(const mat& A)
{
return sqrtm(to_cmat(A));
}