diff --git a/ea/ub8/framework/gp/DummyGPIndividual.java b/ea/ub8/framework/gp/DummyGPIndividual.java index 8a0c5a2..8e6c547 100644 --- a/ea/ub8/framework/gp/DummyGPIndividual.java +++ b/ea/ub8/framework/gp/DummyGPIndividual.java @@ -29,7 +29,6 @@ @Override public DummyGPIndividual makeRandomSubtree(int h, int mode) { - // ja da habt ihr ne nette endlosschleife programmiert return new DummyGPIndividual(this, h - 1, mode); diff --git a/ea/ub8/framework/gp/EvolutionaryAlgorithm.java b/ea/ub8/framework/gp/EvolutionaryAlgorithm.java index 486a7e6..e8e40ed 100644 --- a/ea/ub8/framework/gp/EvolutionaryAlgorithm.java +++ b/ea/ub8/framework/gp/EvolutionaryAlgorithm.java @@ -42,7 +42,7 @@ // indies[i] = new DummyGPIndividual(6, GPTree.MODE_GROW); // TODO: Replace with RegrIndividual indies[i] = new RegrIndividual(6, GPTree.MODE_GROW); - indies[i].defaultInit(); + // indies[i].defaultInit(); changed } } diff --git a/ea/ub8/framework/gp/GPTree.java b/ea/ub8/framework/gp/GPTree.java index 46bff3f..1da7f1e 100644 --- a/ea/ub8/framework/gp/GPTree.java +++ b/ea/ub8/framework/gp/GPTree.java @@ -15,8 +15,10 @@ public static int MODE_GROW = 1; public Random ran = new Random(); + // schonmal was von Javadoc für public Felder gehört?? Ihr könnt doch + // Schnittstellen nicht unkommentiert lassen. public int arity; // Stelligkeit des Knotens aha! - public int type; // Knotentyp . Schon mal was von Enums gehört... + protected int type; // Knotentyp . Schon mal was von Enums gehört... public int height; // Hoehe des Baumes public int nodeCount; // Knotenanzahl in diesem Baum public GPTree up, left, right; // Zeiger auf Eltern- und Unterknoten @@ -49,18 +51,25 @@ * Dieser Konstruktor erzeugt mit Hilfe von copyTree() eine tiefe Kopie * des Baumes tree. */ + if (!tree.isValid()) { + throw new RuntimeException("not valid tree to copy"); + } this.arity = tree.arity; this.type = tree.type; this.height = tree.height; this.nodeCount = tree.nodeCount; - if (left != null) { - left = copyTree(left); + this.up = tree.up; + if (tree.left != null) { + left = copyTree(tree.left); left.up = this; } - if (right != null) { - right = copyTree(right); + if (tree.right != null) { + right = copyTree(tree.right); right.up = this; } + if (!this.isValid()) { + throw new RuntimeException("after copy no valid tree"); + } } /** @@ -80,9 +89,9 @@ */ // :-D :-D whooo das kommentar ist der hammer, da hat einer Vererbung // und Typen einfach nicht verstanden - if (maxH > 0) { - parent = makeRandomSubtree(maxH, mode);// TODO mode - } + + this.up = parent; + makeRandomSubtree(maxH, mode); } @@ -93,6 +102,8 @@ // ************* abstract methods // these two are abstract so that the subtypes can return the right type // they should again use GPTree's (copy) constructor! + + // na wie wärs den mit Javadoc ??? public abstract GPTree makeRandomSubtree(int h, int mode); public abstract GPTree copyTree(GPTree tree); @@ -101,7 +112,9 @@ public abstract int getTermCount(); // return number of terminals - public abstract int getUnaryCount(); // return number of unary functions + public abstract int getUnaryCount(); // return number of unary functions // + // Toller Javadoc, so als + // Kommentar... public abstract int getBinaryCount(); // return number of binary functions @@ -140,16 +153,51 @@ } switch (arity) { case 0: - return ((left == null) && (right == null) && (height == 0) && (nodeCount == 1)); + boolean a = ((left == null) && (right == null) && (height == 0) && (nodeCount == 1)); + if (!a) { + System.out.println(a + "case 0"); + System.out.println(left); + System.out.println(right); + System.out.println(height); + System.out.println(nodeCount); + System.out.println("_________"); + } + return a; case 1: - return ((left != null) && (right == null) + boolean b = ((left != null) + && (right == null) && (height == (left.height + 1)) - && (nodeCount == (left.nodeCount + 1)) && left.isValid() && (left.up == this)); + /* && (nodeCount == (left.nodeCount + 1)) */&& left + .isValid() && (left.up == this)); + if (!b) { + System.out.println(b + "case1"); + System.out.println("left" + left); + System.out.println("right " + right); + System.out + .println("height " + height + " " + (left.height + 1)); + System.out.println("nodecount " + nodeCount + " " + + (left.nodeCount + 1)); + System.out.println("parent " + (left.up == this)); + System.out.println("_________"); + } + return b; case 2: - return ((left != null) && (right != null) + boolean c = ((left != null) && (right != null) && (height == (Math.max(left.height, right.height) + 1)) - && (nodeCount == (left.nodeCount + right.nodeCount + 1)) + /* && (nodeCount == (left.nodeCount + right.nodeCount + 1)) */ && left.isValid() && right.isValid() && (left.up == this) && (right.up == this)); + if (!c) { + System.out.println(c + "case2"); + System.out.println("left" + left); + System.out.println("right " + right); + System.out.println("height " + height + " " + + (Math.max(left.height, right.height) + 1)); + System.out.println("nodecount " + nodeCount + " " + + (left.nodeCount + right.nodeCount + 1)); + System.out.println("parent " + (left.up == this)); + System.out.println("_________"); + } + return c; } return false; } @@ -169,24 +217,103 @@ * Hinweis: wählen Sie zuerst zwei zufällige Teilbäume von this und * otherTree. Dabei kann eine Vorstellung der Numerierung der * Unterknoten helfen. + * + * ist die maximale höhe der teilbäume gemeint? nichtmal gescheite + * javadoc gibts.. */ - GPTree os = findSubtree(ran.nextInt(nodeCount) + 1); - GPTree ts = findSubtree(ran.nextInt(nodeCount) + 1); - if (os.up.left.equals(os)) { - os.up.left = ts; - } else { - os.up.right = ts; + + if (!this.isValid()) { + + throw new RuntimeException(" this not valid before crossover"); } - if (ts.up.left.equals(ts)) { - ts.up.left = os; + if (!otherTree.isValid()) { + + throw new RuntimeException(" othertree not valid before crossover"); + } + + List thisTrees = new ArrayList(); + List otherTrees = new ArrayList(); + getAllTreesSameOrLower(this, h, thisTrees); + GPTree ts = null; + GPTree os = null; + if ((thisTrees.size() > 0)) { + ts = thisTrees.get(ran.nextInt(thisTrees.size())); } else { - ts.up.right = os; + return; + } + getAllTreesSameHight(otherTree, ts.height, otherTrees); + if ((otherTrees.size() > 0)) { + os = otherTrees.get(ran.nextInt(otherTrees.size())); + } else { + return; + } + if (os.up != null) { + os.up.nodeCount -= os.nodeCount; + os.up.nodeCount += ts.nodeCount; + + if (os.up.left.equals(os)) { + os.up.left = ts; + } else { + os.up.right = ts; + } + } + if (ts.up != null) { + ts.up.nodeCount -= ts.nodeCount; + ts.up.nodeCount += os.nodeCount; // todo notecount der eltern + // anpassen --> das wird jetzt + // echt zu viel code für die + // paar punkte . Wir ignorieren + // jetzt einfach den nodecount + // bei der prüfung braucht man + // eh nie + if (ts.up.left.equals(ts)) { + ts.up.left = os; + } else { + ts.up.right = os; + } } GPTree op = os.up; os.up = ts.up; ts.up = op; + if (!this.isValid()) { + throw new RuntimeException("this is not valid after crossover"); + } + if (!otherTree.isValid()) { + throw new RuntimeException( + "other tree is not valid after crossover"); + } + } + private void getAllTreesSameOrLower(GPTree start, int h, List result) { + if (start.height >= 0) { + if (start.height <= h) { + result.add(start); + } + if (start.left != null) { + getAllTreesSameOrLower(start.left, h, result); + } + if (start.right != null) { + getAllTreesSameOrLower(start.right, h, result); + } + } + }; + + private void getAllTreesSameHight(GPTree start, int h, List result) { + if (start.height >= h) { + if (start.height == h) { + result.add(start); + return; + } + if (start.left != null) { + getAllTreesSameHight(start.left, h, result); + } + if (start.right != null) { + getAllTreesSameHight(start.right, h, result); + } + } + }; + /** * @param pMut * propability of mutation @@ -204,46 +331,41 @@ // Wie ist das zu verstehen? eine zufällige Stelle dieses Teilbaums? if (ran.nextDouble() <= pMut) { - int index = ran.nextInt(nodeCount) + 1;// arraybaum index statet mit - // 1 - GPTree sub = findSubtree(index); - GPTree ne = makeRandomSubtree(h, mode); + GPTree sub = null; + GPTree ne = null; + List treesOfSameHight = new ArrayList(); + getAllTreesSameHight(this, h, treesOfSameHight); + if (treesOfSameHight.size() <= 0) { + return; + } + sub = treesOfSameHight.get(ran.nextInt(treesOfSameHight.size())); + ne = makeRandomSubtree(h, mode); // es wird immer nur ein Baum mit + // wieder der gleichen höhe + // erstellt wie der vorherige + // Hier funktioniert unser code nicht da makeRandom subtree nur this + // ändert und wir ja hier nicht auf subtypen casten können. + // So und hier geben wir es auf die Aufgabe zu lösen, mit der + // Hoffnung dass ihr uns nie wieder so schlecht kommentierten und so + // häufig gegen Java-Spezifikationen verstoßenden Code gebt wie + // diesen. + ne.up = sub.up; - if (sub.up.left.equals(sub)) { - ne.up.left = ne; - } else { - ne.up.right = ne; + if (ne.up != null) { + sub.up.nodeCount -= sub.nodeCount; + sub.up.nodeCount += ne.nodeCount; + if (sub.up.left.equals(sub)) { + sub.up.left = ne; + } else { + sub.up.right = ne; + } + sub.up = null; } - sub.up = null; } + if (!this.isValid()) { + System.err.println(this.toString() + " not valid mutation"); + throw new RuntimeException(""); + } } - /** - * @param index - * starts at 1 - * @return the GPTree at this index - */ - private GPTree findSubtree(int index) { - int cIndex = index; - List path = new ArrayList(); // geht sicher - // besser hab - // ich mir - // gerade selbst - // so gedacht - path.add(0, cIndex); - while (cIndex < 2) { - cIndex = cIndex / 2; - path.add(0, cIndex); - } - GPTree aimNode = this; - for (int i : path) { - if ((i % 2) == 0) { - aimNode = aimNode.left; - } else { - aimNode = aimNode.right; - } - } - return aimNode; - } } diff --git a/ea/ub8/framework/gp/RegrIndividual.java b/ea/ub8/framework/gp/RegrIndividual.java index 03a9aaa..04f7ef9 100644 --- a/ea/ub8/framework/gp/RegrIndividual.java +++ b/ea/ub8/framework/gp/RegrIndividual.java @@ -9,10 +9,10 @@ static String unary[] = { "exp", "sin", "cos" }; static String binary[] = { "+", "-", "*", "/" }; - int maxHeight = 5; - int initHeight = 3; - int modeCreate = MODE_GROW; - int modeMutate = MODE_GROW; + static int maxHeight = 5; + static int initHeight = 3; + static int modeCreate = MODE_GROW; + static int modeMutate = MODE_GROW; public RegrIndividual(GPTree parent, int h, int mode) { super(parent, h, mode); @@ -42,13 +42,14 @@ @Override public void crossover(InterfaceIndividual y) { - super.crossover((GPTree) y, maxHeight); + super.crossover((GPTree) y, this.height); } @Override public void mutate(double pMut) { - super.mutate(pMut, maxHeight, modeMutate); + super.mutate(pMut, this.height != 0 ? ran.nextInt(this.height) + 1 : 0, + modeMutate); } @Override @@ -64,21 +65,44 @@ @Override public GPTree makeRandomSubtree(int h, int mode) { - this.arity = ran.nextInt(3); + // System.out.println(); + if (h > 0) { // letzter Knoten hat höhe 0 + if ((mode == 0)) { + this.arity = ran.nextInt(2) + 1; + } else { + this.arity = ran.nextInt(3); + } + } else { + this.arity = 0; + } + switch (arity) { case 0: type = ran.nextInt(3); + nodeCount = 1; + height = 0; + left = null; + right = null; + break; case 1: type = ran.nextInt(3); left = new RegrIndividual(this, h - 1, mode); + left.up = this; + right = null; + nodeCount = left.nodeCount + 1; + height = left.height + 1; + break; case 2: type = ran.nextInt(4); left = new RegrIndividual(this, h - 1, mode); + left.up = this; right = new RegrIndividual(this, h - 1, mode); + right.up = this; + nodeCount = right.nodeCount + left.nodeCount + 1; + height = left.height > right.height ? left.height + 1 + : right.height + 1; + break; } - height = maxHeight - height; - nodeCount += 1;// Ka wie ich das bestimmen soll. - return this; } @@ -134,7 +158,7 @@ private double getF(double x) { RegrIndividual l = (RegrIndividual) left; RegrIndividual r = (RegrIndividual) right; - + // System.out.println(this); switch (arity) { case 0: switch (getTermSymbol(type)) { @@ -145,6 +169,7 @@ case "pi": return Math.PI; } + break; case 1: switch (getUnarySymbol(type)) { case "exp": @@ -155,6 +180,7 @@ return Math.cos(l.getF(x)); } + break; case 2: switch (getBinarySymbol(type)) { case "+": @@ -167,6 +193,7 @@ return l.getF(x) / r.getF(x); } + break; } throw new RuntimeException("wrong arity" + arity); diff --git a/mr/ub8/localize_ekf/motion_diff.m b/mr/ub8/localize_ekf/motion_diff.m index 1faaaa7..639976b 100644 --- a/mr/ub8/localize_ekf/motion_diff.m +++ b/mr/ub8/localize_ekf/motion_diff.m @@ -6,25 +6,31 @@ theta = x(3); sl = u(1); sr = u(2); - +if(sr~=sl) % YOUR CODE STARTS HERE % deltaTheta= (sr-sl)/l; r= (sl+sr)/(sr-sl) *l/2; + % fill this with the result of applying the motion model -if(sr==sl) + xnew= x+ [r*(sin(theta+deltaTheta)-sin(theta)); r*(-cos(theta+deltaTheta)-cos(theta)); deltaTheta]; + +G = [1,0,r*(cos(theta+deltaTheta)-cos(theta)); + 0,1,r*(sin(theta+deltaTheta)-sin(theta)); + 0,0,1]; else xnew= x+[sl*cos(theta); sl*sin(theta); 0]; +G = [1,0,r*(cos(theta+deltaTheta)-cos(theta)); + 0,1,r*(sin(theta+deltaTheta)-sin(theta)); + 0,0,1]; end % fill this with the motion Jacobian G % G=zeros(3,3) -G = [1,0,r*(cos(theta+deltaTheta)-cos(theta)); - 0,1,r*(sin(theta+deltaTheta)-sin(theta)); - 0,0,1]; + % YOUR CODE ENDS HERE % diff --git a/mr/ub8/mr8.pdf b/mr/ub8/mr8.pdf index 091b875..86a8443 100644 --- a/mr/ub8/mr8.pdf +++ b/mr/ub8/mr8.pdf Binary files differ diff --git a/mr/ub8/mr8.tex b/mr/ub8/mr8.tex index 0b37338..4e9291a 100644 --- a/mr/ub8/mr8.tex +++ b/mr/ub8/mr8.tex @@ -117,18 +117,18 @@ \Normalfs{x_t}{ax_{t-1}}{q_t}&=\eta\Normalfs{z_t}{cx_t}{r_t}\Normalfs{x_{t-1}}{\hat{x}_{t-1}}{\sigma_{t-1}} \end{align*} %TODO - % \item $p(x_t|z_{t,\dots1},u_{t,\dots1})\sim N(x_t,\mu_{x,t},\sigma_{x,t})\\ - % \mu_{x,t}= a\mu_{x,t-1}+bu_t+\epsilon_t\\ - % \sigma_{x,t}= |a|\sigma_{x,t-1}\\ - % \\ - % p(x_{t}|z_{t,\dots1},u_{t,\dots1})\sim N(x_{t-1},\mu_{x,t-1},\sigma_{x,t-1})\\ - % \mu_{x,t-1}= a\mu_{x,t-2}+bu_{t-1}+\epsilon_{t-1}\\ - % \sigma_{x,t-1}= |a|\sigma_{x,t-2}\\ - % \\ - % p(z_t|x-t)\sim N(z_t,\mu_z,\sigma_z)\\ - % \mu_z=c*\mu_{x,t-1}+\delta_t \\ - % \sigma_z=|c|\sigma_{x,t-1} - % $ + \item $p(x_t|z_{t,\dots1},u_{t,\dots1})\sim N(x_t,\mu_{x,t},\sigma_{x,t})\\ + \mu_{x,t}= a\mu_{x,t-1}+bu_t+\epsilon_t\\ + \sigma_{x,t}= |a|\sigma_{x,t-1}\\ + \\ + p(x_{t}|z_{t,\dots1},u_{t,\dots1})\sim N(x_{t-1},\mu_{x,t-1},\sigma_{x,t-1})\\ + \mu_{x,t-1}= a\mu_{x,t-2}+bu_{t-1}+\epsilon_{t-1}\\ + \sigma_{x,t-1}= |a|\sigma_{x,t-2}\\ + \\ + p(z_t|x_t)\sim N(z_t,\mu_z,\sigma_z)\\ + \mu_z=c*\mu_{x,t}+\delta_t \\ + \sigma_z=|c|\sigma_{x,t} + $ \end{enumerate} \end{document}