public FilterResult PrtFltr_MkwskSum( int max_iter, double epsilon, SistemaObstaculos S, List <PointD> arm, PointD obj, int samples, double media_gausiana) { int it; double distance; var parametros_gausiana = new GausianParameters(media_gausiana, 0, 0, 1); var chaint = new Chain(arm.ToArray()); var particles = new Particles(chaint, samples, parametros_gausiana.ToArray(), true); Particle CHresult = pik(max_iter, epsilon, obj, particles, S, out it, out distance); PointD[] chainResult_points = CHresult.Cadena.Chain2positions(); var fr = new FilterResult { ChainResult = CHresult, ChainResult_points = chainResult_points, Iterations = it, Distance = distance }; return(fr); }
private static Particles PF(PointD goal, Particles particles, SistemaObstaculos S, out int bestIndex) { double wSum = 0; double resampling = 0; var p = new Particles(); for (int i = 0; i < particles.Particulas.Count; i++) { //sampling //for (int j = 0; j < particles.Particulas[i].Cadena.Size; j++ ) //p.Particulas[i].Cadena = Chain c = Chain.ChainQuTEMSampling(particles.Particulas[i].Gausian, particles.Particulas[i].Cadena); //??????? //compute weight //p.Particulas[i].Gausian = double[] gausian = particles.Particulas[i].Gausian; double weight = Particle.WeightFunction(c, goal, S); //p.Particulas[i].W = double w = particles.Particulas[i].W * weight; //weights cumsum var particle = new Particle(c, gausian, w); p.Particulas.Add(particle); wSum = wSum + w; //computing resamplig condition resampling = resampling + Math.Pow(w, 2); } //normalize weights foreach (Particle t in p.Particulas) { t.W = t.W / wSum; } // best chain double max = 0; int imax = 0; for (int i = 0; i < p.Particulas.Count; i++) { //ww = particles(i).w if (p.Particulas[i].W > max) { max = p.Particulas[i].W; imax = i; } } bestIndex = imax; if (p.Particulas[bestIndex].W == 0) { bestIndex = 0; } //if necessary, resampling //Effective Sample Size, Tutorial PF, 15 year later // if ((1/resampling) <= (size(p, 1)/2)) // 'resampling' return(Particles.Remostrejar(p)); }
//public double WeightFunction(PointD goal, SistemaObstaculos S) //{ // // euclideanDistance // PointD ee = this.Cadena.ChainEndEffectorPosition(); // //Si ms de un end effector, multiplicar // //w = exp((-1)*(euclidianDistance(ee, goal))*imageDistance(image, chain)); // double w = Math.Exp((-1)*MathUtils.euclidianDistance(ee, goal)); // //el peso será cero si entra en contacto con algun obstáculo // w = w * S.S_Touch(this.Cadena); // return w; //} public static double WeightFunction(Chain chain, PointD goal, SistemaObstaculos S) { // euclideanDistance PointD ee = chain.ChainEndEffectorPosition(); //Si ms de un end effector, multiplicar //w = exp((-1)*(euclidianDistance(ee, goal))*imageDistance(image, chain)); double w = Math.Exp((-1) * MathUtils.euclidianDistance(ee, goal)); //el peso será cero si entra en contacto con algun obstáculo w = w * S.S_Touch(chain); return(w); }
private Particle pik(int max_iter, double epsilon, PointD goal, Particles particles, SistemaObstaculos S, out int it, out double distance) { //chainResult = chain; Particle chainResult = particles.Particulas[0]; PointD ee = chainResult.Cadena.ChainEndEffectorPosition(); it = 0; while (it < max_iter && MathUtils.euclidianDistance(ee, goal) > epsilon) { int I; particles = PF(goal, particles, S, out I); ee = particles.Particulas[I].Cadena.ChainEndEffectorPosition(); chainResult = particles.Particulas[I]; it = it + 1; } distance = MathUtils.euclidianDistance(ee, goal); return(chainResult); }