static void Main(string[] args) { Function.newContext(); Function.printCompilerSource = true; //Function.turnOnAllSimplification(); Program prog = new Program(); //int T = 10; //Function[][] X = new Function[T][]; //Function[][] U = new Function[T-1][]; //Function[] x = new Function[] {-4, 2}; //Function[] u = new Function[] {0.214285714285714, -0.285714285714286}; //Function[,] Sigma = new Function[,] { { 1, 0 }, { 0, 1 } }; Variable[] vars = new Variable[8]; for (int i = 0; i < 8; ++i) { vars[i] = new Variable("vars_" + i); } Function[] x = new Function[] { vars[0], vars[1] }; Function[] u = new Function[] { vars[2], vars[3] }; Function[,] Sigma = VM.identity <Function>(2, 0, 1); Function[] q_t = new Function[] { vars[4], vars[5] }; Function[] r_t = new Function[] { vars[6], vars[7] }; Function[] x_tp1; Function[,] Sigma_tp1; prog.EKF(x, Sigma, u, q_t, r_t, out x_tp1, out Sigma_tp1); Function[] packed = VM.concatenate <Function>(x_tp1, VM.matrixToVector <Function>(Sigma_tp1)); Function beliefvec = Function.derivative(packed); beliefvec.printOperatorCounts(); bool[] invDynInputVarIndices; vars = initializeInputVariables(beliefvec, vars, out invDynInputVarIndices); beliefvec.orderVariablesInDomain(vars); RuntimeFunction brun = beliefvec.compile(); double[] result = new double[6], varvals = { -4, 2, 0.214285714285714, -0.285714285714286, 0, 0 }; brun.eval(result, varvals); for (int i = 0; i < 6; ++i) { Console.Write("b[" + i + "]: " + result[i] + " "); } Console.WriteLine(); Console.Read(); }
// one EKF time step check void testEKFStep() { int nvars = 12; Variable[] vars = new Variable[nvars]; for (int i = 0; i < nvars; ++i) { vars[i] = new Variable("vars_" + i); } Function[] x = new Function[] { vars[0], vars[1] }; Function[] u = new Function[] { vars[2], vars[3] }; Function[] q = new Function[] { vars[4], vars[5] }; Function[] r = new Function[] { vars[6], vars[7] }; Function[,] Sigma = new Function[, ] { { vars[8], vars[9] }, { vars[10], vars[11] } }; Function[] x_tp1; Function[,] Sigma_tp1; EKF(x, u, q, r, Sigma, out x_tp1, out Sigma_tp1); Function[] packed = VM.concatenate <Function>(x_tp1, VM.matrixToVector <Function>(Sigma_tp1)); Function beliefvec = Function.derivative(packed); beliefvec.printOperatorCounts(); bool[] inputVarIndices; vars = initializeInputVariables(beliefvec, vars, out inputVarIndices); beliefvec.orderVariablesInDomain(vars); RuntimeFunction brun = beliefvec.compile(); double[] result = new double[6], varvals = { -4, 2, 0.214285714285714, -0.285714285714286, 0, 0, 1, 0, 0, 1 }; brun.eval(result, varvals); for (int i = 0; i < 6; ++i) { Console.Write("b[" + i + "]: " + result[i] + " "); } Console.WriteLine(); }
// TODO! void testBeliefUpdate() { // num timesteps int T = 15; // variable instantiations int nparams = 3; int nvars = T * XDIM + (T - 1) * UDIM + QDIM + RDIM + (XDIM * XDIM) + nparams; Variable[] vars = new Variable[nvars]; for (int i = 0; i < nvars; ++i) { vars[i] = new Variable("vars_" + i); } int idx = initVars(vars); Function alpha_belief = vars[idx++]; Function alpha_control = vars[idx++]; Function alpha_final_belief = vars[idx++]; Function[] x_T; Function[,] Sigma_T; beliefUpdate(X, U, q, r, Sigma_0, out x_T, out Sigma_T); Function[] packed = VM.concatenate <Function>(x_T, VM.matrixToVector <Function>(Sigma_T)); Function beliefvec = Function.derivative(packed); beliefvec.printOperatorCounts(); bool[] inputVarIndices; vars = initializeInputVariables(beliefvec, vars, out inputVarIndices); beliefvec.orderVariablesInDomain(vars); RuntimeFunction beliefrun = beliefvec.compile(); //beliefrun.compileCCodeToFile("test.c"); // For evaluation double[] varvals = new double[nvars]; double[] inputvarvals = new double[beliefrun.domainDimension]; idx = initVarVals(T, varvals); int inputidx = 0; for (int i = 0; i < nvars; ++i) { if (inputVarIndices[i]) { inputvarvals[inputidx++] = varvals[i]; } } double[] result = new double[6]; beliefrun.eval(result, inputvarvals); for (int i = 0; i < 6; ++i) { Console.Write("b[" + i + "]: " + result[i] + " "); } Console.WriteLine(); }
// one EKF time step check void testEKFStep() { int nvars = XDIM + UDIM + QDIM + RDIM + XDIM * XDIM; Variable[] vars = new Variable[nvars]; for (int i = 0; i < nvars; ++i) { vars[i] = new Variable("vars_" + i); } Function[] x = new Function[XDIM]; Function[] u = new Function[UDIM]; Function[] q = new Function[QDIM]; Function[] r = new Function[RDIM]; Function[,] Sigma = new Function[XDIM, XDIM]; int idx = 0; for (int i = 0; i < XDIM; ++i) { x[i] = vars[idx++]; } for (int i = 0; i < UDIM; ++i) { u[i] = vars[idx++]; } for (int i = 0; i < QDIM; ++i) { q[i] = vars[idx++]; } for (int i = 0; i < RDIM; ++i) { r[i] = vars[idx++]; } for (int i = 0; i < XDIM; ++i) { for (int j = 0; j < XDIM; ++j) { Sigma[i, j] = vars[idx++]; } } Function[] x_tp1; Function[,] Sigma_tp1; EKF(x, u, q, r, Sigma, out x_tp1, out Sigma_tp1); Function[] packed = VM.concatenate <Function>(x_tp1, VM.matrixToVector <Function>(Sigma_tp1)); Function beliefvec = Function.derivative(packed); beliefvec.printOperatorCounts(); bool[] inputVarIndices; vars = initializeInputVariables(beliefvec, vars, out inputVarIndices); beliefvec.orderVariablesInDomain(vars); RuntimeFunction brun = beliefvec.compile(); double[] result = new double[nvars]; double[] varvals = new double[nvars]; double[] joints = { Math.PI / 2, -1.5431281995798991, -0.047595544887998331, 1.4423058659586809, 1.5334368368992011, -1.1431255223182604 }; double[] input = { -.1, .1, -.1, -.1, -.1, .1 }; double[] x_noise = { 0, 0, 0, 0, 0, 0 }; double[] z_noise = { 0, 0, 0, 0 }; double[] S = new double[XDIM * XDIM]; for (int i = 0; i < XDIM; ++i) { for (int j = 0; j < XDIM; ++j) { if (i == j) { S[i + j * XDIM] = 1; } else { S[i + j * XDIM] = 0; } } } Console.WriteLine("Before copying"); joints.CopyTo(varvals, 0); input.CopyTo(varvals, XDIM); x_noise.CopyTo(varvals, XDIM + UDIM); z_noise.CopyTo(varvals, XDIM + UDIM + QDIM); S.CopyTo(varvals, XDIM + UDIM + QDIM + RDIM); Console.WriteLine("After copying"); brun.eval(result, varvals); printBelief(result); }
void testKinematics() { double[] xtest = new double[XDIM]; xtest[0] = Math.PI / 2; xtest[1] = -1.5431281995798991; xtest[2] = -0.047595544887998331; xtest[3] = 1.4423058659586809; xtest[4] = 1.5334368368992011; xtest[5] = -1.1431255223182604; analyticalJacobian(xtest); int nvars = 6; Variable[] vars = new Variable[nvars]; Function[] x = new Function[XDIM]; for (int i = 0; i < nvars; ++i) { vars[i] = new Variable("vars_" + i); x[i] = vars[i]; } //Function[,] J, H; Function[,] H; //symbolicJacobian(x, out J, out H); symbolicJacobian(x, out H); Function Hvec = Function.derivative(VM.jaggedToLinear <Function>(VM.toJaggedArray <Function>(H))); Hvec.printOperatorCounts(); bool[] inputVarIndices; vars = initializeInputVariables(Hvec, vars, out inputVarIndices); Hvec.orderVariablesInDomain(vars); RuntimeFunction Hrun = Hvec.compile(); Console.WriteLine(Hrun.domainDimension + " " + Hrun.rangeDimension); double[] result = new double[Hrun.rangeDimension]; Hrun.eval(result, xtest); for (int j = 0; j < Hrun.rangeDimension; ++j) { Console.WriteLine(result[j]); } Console.WriteLine(); /* * Function Jvec = Function.derivative(VM.jaggedToLinear<Function>(VM.toJaggedArray<Function>(J))); * //Jvec.printOperatorCounts(); * * * bool[] inputVarIndices; * vars = initializeInputVariables(Jvec, vars, out inputVarIndices); * for (int i = 0; i < nvars; ++i) * { * if (inputVarIndices[i]) * { * Console.WriteLine("1 "); * } * else * { * Console.WriteLine("0 "); * } * } * * * Jvec.orderVariablesInDomain(vars); * * RuntimeFunction Jrun = Jvec.compile(); * Console.WriteLine(Jrun.domainDimension + " " + Jrun.rangeDimension); * * double[] result = new double[Jrun.rangeDimension]; * Jrun.eval(result, xtest); * for (int j = 0; j < Jrun.rangeDimension; ++j) * { * Console.WriteLine(result[j]); * } * Console.WriteLine(); */ }