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();
*/
}
