void points_update(float value, int state) {
// Value is distance in cm when state is MOVE_STATE
//and degrees when state is ROTATE_STATE
int i;
float e, f;
switch (state) {
case MOVE_STATE:
for(i=0; i<NUMBER_OF_PARTICLES; i++) {
e = sampleGaussian(0.0, 0.881);
f = sampleGaussian(0.0, 0.881);
xArray[i] = xArray[i] + (value + e) * cosDegrees(thetaArray[i]);
yArray[i] = yArray[i] + (value + e) * sinDegrees(thetaArray[i]);
thetaArray[i] = thetaArray[i] + f;
}
break;
case ROTATE_STATE:
for( i=0; i<NUMBER_OF_PARTICLES; i++) {
e = sampleGaussian(0.0, 0.881);
thetaArray[i] = normalize_angle_value(thetaArray[i] + value + e);
}
break;
}
}
tf::Pose MotionModel::updateAction(tf::Pose& p)
{
double delta_rot1;
if (dxy < 0.01)
delta_rot1 = 0.0;
else
delta_rot1 = angle_diff(atan2(delta_y, delta_x), delta_yaw);
double delta_trans = dxy;
double delta_rot2 = angle_diff(delta_yaw, delta_rot1);
double delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)), fabs(angle_diff(delta_rot1, M_PI)));
double delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)), fabs(angle_diff(delta_rot2, M_PI)));
double delta_rot1_hat = angle_diff(delta_rot1, sampleGaussian(alpha1 * delta_rot1_noise*delta_rot1_noise +
alpha2 * delta_trans*delta_trans));
double delta_trans_hat = delta_trans - sampleGaussian(alpha3 * delta_trans*delta_trans +
alpha4 * delta_rot1_noise*delta_rot1_noise +
alpha4 * delta_rot2_noise*delta_rot2_noise);
double delta_rot2_hat = angle_diff(delta_rot2, sampleGaussian(alpha1 * delta_rot2_noise*delta_rot2_noise +
alpha2 * delta_trans*delta_trans));
delta_x = delta_trans_hat * cos(delta_rot1_hat);
delta_y = delta_trans_hat * sin(delta_rot1_hat);
delta_yaw = delta_rot1_hat + delta_rot2_hat;
tf::Pose noisy_pose(tf::createQuaternionFromRPY(0,0,delta_yaw),tf::Vector3(delta_x,delta_y,0));
tf::Transform base_to_global_ = tf::Transform(p.getRotation());
noisy_pose.setOrigin(base_to_global_ * noisy_pose.getOrigin());
p.setOrigin(p.getOrigin() + noisy_pose.getOrigin());
p.setRotation(p.getRotation() * noisy_pose.getRotation());
}
void updateParticleArraysForward(float distanceMoved)
{
for (int particle = 0; particle < NUMBER_OF_PARTICLES; particle++)
{
//float uniform_float = sampleUniform(1.0);
e = sampleGaussian(0.0, 0.005);
f = sampleGaussian(0.0, 0.005);
xArray[particle] = xArray[particle] + (distanceMoved + e)*cos(thetaArray[particle]);
yArray[particle] = yArray[particle] + (distanceMoved + e)*sin(thetaArray[particle]);
thetaArray[particle] = thetaArray[particle] + f;
}
}
Transformation3D Odometry::getIncrement(const Odometry& lastOdometry,float noise)
{
Pose3D inc=lastOdometry.pose.inverted()*pose;
if(noise>0){
double r,p,y;
inc.orientation.getRPY(r,p,y);
double m=inc.module();
Pose3D noisePose(m*sampleGaussian(0,noise),m*sampleGaussian(0,noise),0,
0,0,m*sampleGaussian(0,noise));
inc*=noisePose;
}
return inc;
}
void updateParticlesForward()
{
float e, f, theta;
float D = 10;
for (int i=0; i < NUM_PARTICLES; i++)
{
e = sampleGaussian(0.0, 0.01);
f = sampleGaussian(0.0, 0.02);
theta = particles[i][2];
particles[i][0] += (D+e)*cos(theta);
particles[i][1] += (D+e)*sin(theta);
particles[i][2] += f;
}
}
task main()
{
eraseDisplay();
// Array for accumulating Gaussian values
int accumulator[100];
for (int i = 0; i < 99; i++) {
accumulator[i] = 0;
}
// Infinite loop
while(1)
{
// Get and print uniform-distributed and Gaussian-distributed random numbers
float uniform_float = sampleUniform(1.0);
float gaussian_float = sampleGaussian(0.0, 1.0);
nxtDisplayString(1, "Uniform : %04f", uniform_float);
nxtDisplayString(2, "Gaussian: %04f", gaussian_float);
// Build a histogram of Gaussian numbers and plot on the NXT screen
// Scale random number to screen resolution
int gaussian_int = (int) (gaussian_float * 10.0) + 50;
accumulator[gaussian_int]++;
nxtSetPixel(gaussian_int, accumulator[gaussian_int]);
wait1Msec(10);
}
}
void updateParticleArraysForward(float distanceMoved)
{
float e = 0;
float f = 0;
for (int particle = 0; particle < NUMBER_OF_PARTICLES; particle++)
{
e = sampleGaussian(0.0, 0.5); // was 1
f = sampleGaussian(0.0, 0.08);
xArray[particle] = xArray[particle] + (distanceMoved + e)*sin(thetaArray[particle]);
yArray[particle] = yArray[particle] + (distanceMoved + e)*cos(thetaArray[particle]);
thetaArray[particle] += f;
}
}
void updateParticleArraysRotate(float degTurned)
{
for (int particle = 0; particle < NUMBER_OF_PARTICLES; particle++)
{
float g = sampleGaussian(0.0, 0.005);
thetaArray[particle] = thetaArray[particle] + (degTurned + g);
}
}
void updateParticlesRotate(float a)
{
float g;
for (int i=0; i < NUM_PARTICLES; i++)
{
g = sampleGaussian(0.0, 0.01);
particles[i][2] += (a + g);
}
}
tf::Pose MotionModel::drawFromMotion(tf::Pose& p)
{
double sxy=0.3*srr;
delta_x+=sampleGaussian(srr*fabs(delta_x)+str*fabs(delta_yaw)+sxy*fabs(delta_y));
delta_y+=sampleGaussian(srr*fabs(delta_y)+str*fabs(delta_yaw)+sxy*fabs(delta_x));
delta_yaw+=sampleGaussian(stt*fabs(delta_yaw)+srt*dxy);
delta_yaw=fmod(delta_yaw, 2*M_PI);
if (delta_yaw>M_PI)
delta_yaw-=2*M_PI;
tf::Pose noisy_pose(tf::createQuaternionFromRPY(0,0,delta_yaw),tf::Vector3(delta_x,delta_y,0));
tf::Transform base_to_global_ = tf::Transform(p.getRotation());
noisy_pose.setOrigin(base_to_global_ * noisy_pose.getOrigin());
p.setOrigin(p.getOrigin() + noisy_pose.getOrigin());
p.setRotation(p.getRotation() * noisy_pose.getRotation());
return p;
}
static void sample(T* tests, int cnt, int patch_sz, int type)
{
switch (type)
{
case 0: // uniform random, i.i.d.
sampleUniform(tests, patch_sz, cnt);
break;
case 1: // Gaussian random, i.i.d.
sampleGaussian(tests, patch_sz, cnt);
break;
default:
STDOUT_ERROR("Bad sample type");
}
}
/************************************************************************
* randomizeCell() *
* Does type-specific initialization for an existing cell just *
* entering the simulated compartment, as opposed to a new daughter *
* cell; uses value ranges associated with randFlag rather than *
* initFlag *
* *
* Parameters *
* Cell *pc: specific cell affected *
* *
* Returns - nothing *
************************************************************************/
void CellType::randomizeCell(Cell *pc)
{
// allocate memory for internal variables
pc->setNumAttributes(attributes.size());
// set real values according to CellType's parameters for random values
for (unsigned int i=0; i<attributes.size(); i++)
switch (attributes[i].m_randFlag)
{
case FIXED:
pc->setValue(i, attributes[i].m_rand1);
break;
case UNIFORM: // rand1 is min, rand2 is max
pc->setValue(i, (attributes[i].m_rand2 -
attributes[i].m_rand1)*RandK::randk() + attributes[i].m_rand1);
break;
case GAUSSIAN: // rand1 is mean, rand2 is std. dev.
pc->setValue(i, sampleGaussian(attributes[i].m_rand1,
attributes[i].m_rand2));
break;
case LOGNORMAL: // rand1 is mean, rand2 is std. dev. of Gaussian
pc->setValue(i, exp(sampleGaussian(attributes[i].m_rand1,
attributes[i].m_rand2)));
break;
}
// if this is a mobile cell, set initial orientation randomly
if (m_speed)
{
double x = 2*RandK::randk() - 1;
double y = 2*RandK::randk() - 1;
double z = 2*RandK::randk() - 1;
double sf = 1 / sqrt( x*x + y*y + z*z );
pc->setDirection(SimPoint(x*sf, y*sf, z*sf));
}
}
//////GAUSSIAN SAMPLER
Sample GaussianSampler::getNextSample()
{
int number=iteration;
double cellsize=0.5F, cov;
while(number>0){
number/=L_n;
cellsize/=2;
}
if(iteration==0)cellsize=0.25;
cov=cellsize*cellsize;
UniformSampler::getNextSample();
for(int j=0;j<getDimension();j++)
normalizedSample[j]=sampleGaussian(6,normalizedSample[j],cov);
return computeSample();
}
int main(int argc, char* argv[]) {
srand(time(0));
point_pf::initialize();
Matrix<X_DIM> x0, xGoal;
x0[0] = -3.5; x0[1] = 2;
xGoal[0] = -3.5; xGoal[1] = -2;
SymmetricMatrix<X_DIM> Sigma0 = .01*identity<X_DIM>();
Matrix<U_DIM> u = (xGoal - x0) / (DT*(T-1));
std::vector<Matrix<X_DIM> > P0(M);
for(int m=0; m < M; ++m) {
P0[m] = sampleGaussian(x0, Sigma0);
}
std::vector<std::vector<Matrix<X_DIM>> > P_t(T);
std::vector<Matrix<Q_DIM> > dyn_noise;
std::vector<Matrix<R_DIM> > obs_noise;
float sampling_noise;
P_t[0] = P0;
for(int t=0; t < T-1; ++t) {
dyn_noise = sampleGaussianN(zeros<Q_DIM,1>(), Q, M);
obs_noise = sampleGaussianN(zeros<R_DIM,1>(), R, M);
sampling_noise = (1/float(M))*(rand() / float(RAND_MAX));
P_t[t+1] = point_pf::beliefDynamics(P_t[t], u, dyn_noise, obs_noise, sampling_noise);
//P_t[t+1] = point_pf::casadiBeliefDynamics(P_t[t], u, dyn_noise, obs_noise, sampling_noise);
}
for(int t=0; t < T; ++t) {
std::cout << "\nt: " << t << "\n";
for(int m=0; m < M; ++m) {
std::cout << ~P_t[t][m];
}
}
point_pf::pythonDisplayParticles(P_t);
return 0;
}
// Add noise to a data matrix.
void GPCMDataReader::addDataNoise(
MatrixXd &data, // Data matrix to add noise to.
double noise // How much noise to add.
)
{
if (noise > 0.0)
{
// First, measure the variance along each channel and scale by noise.
int N = data.rows();
MatrixXd bias = data.colwise().sum()/N;
MatrixXd var = (data - bias.replicate(N,1)).colwise().squaredNorm()*(noise/N);
// Now get normally distributed samples.
MatrixXd samples(data.rows(),1);
VectorXd gcov(1);
for (int i = 0; i < data.cols(); i++)
{
gcov(0) = var(0,i);
sampleGaussian(gcov,samples);
data.col(i) += samples;
}
}
}
OrientedPoint EigenCovariance3::sample() const{
static gsl_matrix * m_evec=NULL;
static gsl_vector * m_noise=NULL;
static gsl_vector * m_pnoise=NULL;
if (m_evec==NULL){
m_evec=gsl_matrix_alloc(3,3);
m_noise=gsl_vector_alloc(3);
m_pnoise=gsl_vector_alloc(3);
}
for (int i=0; i<3; i++){
for (int j=0; j<3; j++)
gsl_matrix_set(m_evec,i,j, evec[i][j]);
}
for (int i=0; i<3; i++){
double v=sampleGaussian(sqrt(eval[i]));
if(isnan(v))
v=0;
gsl_vector_set(m_pnoise,i, v);
}
gsl_blas_dgemv (CblasNoTrans, 1., m_evec, m_pnoise, 0, m_noise);
OrientedPoint ret(gsl_vector_get(m_noise,0),gsl_vector_get(m_noise,1),gsl_vector_get(m_noise,2));
ret.theta=atan2(sin(ret.theta), cos(ret.theta));
return ret;
}
