void EKF(Function[] x_t, Function[] u_t, Function[] q_t, Function[] r_t, Function[,] Sigma_t, out Function[] x_tp1, out Function[,] Sigma_tp1)
{
VM.checkSize <Function>(Sigma_t, XDIM, XDIM);
Function[,] A, M;
computeDynJacobians(x_t, u_t, q_t, out A, out M);
Sigma_tp1 = VM.plus(VM.mult(VM.mult(A, Sigma_t), VM.transpose <Function>(A)), VM.mult(VM.mult(M, Q), VM.transpose <Function>(M)));
//VM.print<Function>(Sigma_tp1);
for (int i = 0; i < QDIM; ++i)
{
q_t[i] = 0;
}
x_tp1 = dynfunc(x_t, u_t, q_t);
//VM.print<Function>(x_tp1);
Function[,] H, N;
computeObsJacobians(x_tp1, r_t, out H, out N);
Function[,] K1 = VM.mult(Sigma_tp1, VM.transpose <Function>(H));
Function[,] K2 = VM.plus(VM.mult(VM.mult(H, Sigma_tp1), VM.transpose <Function>(H)), VM.mult(VM.mult(N, R), VM.transpose <Function>(N)));
Function[,] K = VM.transpose(mdivide(VM.transpose(K2), VM.transpose(K1)));
Sigma_tp1 = VM.mult(VM.minus(VM.identity <Function>(XDIM, 0, 1), VM.mult(K, H)), Sigma_tp1);
//VM.print<Function>(Sigma_tp1);
}
public Program()
{
DT = 1;
XDIM = 2; UDIM = 2; ZDIM = 2; QDIM = 2; RDIM = 2;
Q = VM.identity <Function>(QDIM, 0, 1);
R = VM.identity <Function>(RDIM, 0, 1);
}
void computeObsJacobians(Function[] x_tp1, Function[] r_t, out Function[,] H, out Function[,] N)
{
// H = d(obsfunc)/dx
H = new Function[ZDIM, XDIM];
for (int i = 0; i < ZDIM; ++i)
{
Function[] Hicol = Function.D(obsfunc(x_tp1, r_t), x_tp1[i]);
for (int j = 0; j < XDIM; ++j)
{
H[i, j] = Hicol[j];
}
}
// N = d(obsfunc)/dr
N = new Function[ZDIM, RDIM];
for (int i = 0; i < ZDIM; ++i)
{
Function[] Nicol = Function.D(obsfunc(x_tp1, r_t), r_t[i]);
for (int j = 0; j < RDIM; ++j)
{
N[i, j] = Nicol[j];
}
}
// testing
//VM.print<Function>(N);
//Function[,] Nd = Function.derivative(N);
//VM.print<Function>(Nd);
}
// J = T from the paper
void symbolicJacobian(Function[] x, out Function[,] H)
{
/*
* // J = dg/dx
* J = new Function[DIM,XDIM];
* for (int i = 0; i < XDIM; ++i)
* {
* Function[] Jicol = Function.D(g(x), x[i]);
* for (int j = 0; j < DIM; ++j)
* {
* J[j,i] = Jicol[j];
* }
* }
*/
// H = d(obsfunc)/dx
H = new Function[ZDIM, XDIM];
Function[] r = new Function[RDIM];
for (int i = 0; i < RDIM; ++i)
{
r[i] = 0;
}
for (int i = 0; i < XDIM; ++i)
{
Function[] Hicol = Function.D(obsfunc(x, r), x[i]);
for (int j = 0; j < ZDIM; ++j)
{
H[j, i] = Hicol[j];
}
}
}
public Automat()
{
#region Initializing
shiftFlag = true;
LastOperator = string.Empty;
StackCellIndex = 0;
CurrentIndex = 0;
Alphabet = new List <string>
{
"+", "*", "C", "~"
};
StateList = new List <string>
{
"<S>", "<E>", "~", "{слож}", "{умнож}", "{ответ}"
};
StackComputor = new Stack <string>();
StackWorker = new Stack <string>();
StackWorker.Push("<S>");
ChainletWorker = new List <string>();
FunctionTable = new Function[6, 4]
{
{ ChangeRule1, ChangeRule1, ChangeRule1, Error },
{ ChangeRule2, ChangeRule3, ChangeRule4, Error },
{ Error, Error, Error, Confirm },
{ ChangeRule4, ChangeRule4, ChangeRule4, ChangeRule4 },
{ ChangeRule4, ChangeRule4, ChangeRule4, ChangeRule4 },
{ ChangeRule4, ChangeRule4, ChangeRule4, ChangeRule4 }
};
#endregion
}
Function[,] mdivide(Function[,] A, Function[,] B)
{
// Cholesky factorization A = L*~L
// check if symmetric
bool check = VM.checkSymmetric <Function>(A);
int size = A.GetLength(0);
Function[,] L = VM.constant <Function>(size, size, 0);
for (int i = 0; i < size; ++i)
{
for (int j = i; j < size; ++j)
{
Function sum = A[j, i];
for (int k = 0; k < i; ++k)
{
sum -= L[j, k] * L[i, k];
}
if (i == j)
{
L[i, i] = Function.sqrt(sum);
}
else
{
L[j, i] = sum / L[i, i];
}
}
}
int ncols = B.GetLength(1);
// Backward and forward substitution
Function[,] M = new Function[size, ncols];
for (int i = 0; i < size; ++i)
{
for (int k = 0; k < ncols; ++k)
{
Function sum = B[i, k];
for (int j = 0; j < i; ++j)
{
sum -= L[i, j] * M[j, k];
}
M[i, k] = sum / L[i, i];
}
}
for (int i = size - 1; i != -1; --i)
{
for (int k = 0; k < ncols; ++k)
{
Function sum = M[i, k];
for (int j = i + 1; j < size; ++j)
{
sum -= L[j, i] * M[j, k];
}
M[i, k] = sum / L[i, i];
}
}
return(M);
}
public Program()
{
DT = 0.5;
DIM = 3; XDIM = 6; UDIM = 6; ZDIM = 4; QDIM = 6; RDIM = 4;
Q = VM.identity <Function>(QDIM, 0, 1);
R = VM.identity <Function>(RDIM, 0, 1);
l4 = 2.375; l3 = 10.375; l2 = 8; l1 = 7.25;
cam0 = new double[DIM]; cam1 = new double[DIM];
cam0[0] = -4; cam0[1] = 23; cam0[2] = 0;
cam1[0] = 4; cam1[1] = 23; cam1[2] = 0;
}
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();
}
public Point(int timesteps)
{
T = timesteps;
DT = 1;
XDIM = 2;
UDIM = 2;
ZDIM = 2;
QDIM = 2;
RDIM = 2;
Q = VM.identity <Function>(QDIM, 0, 1);
R = VM.identity <Function>(RDIM, 0, 1);
}
void beliefUpdate(Function[][] X, Function[][] U, Function[] q, Function[] r, Function[,] Sigma_0, out Function[] x_T, out Function[,] Sigma_T)
{
int T = X.GetLength(0);
Function[] x_tp1 = X[0];
Function[,] Sigma_t = Sigma_0, Sigma_tp1;
for (int t = 0; t < T - 1; ++t)
{
EKF(X[t], U[t], q, r, Sigma_t, out x_tp1, out Sigma_tp1);
Sigma_t = Sigma_tp1;
}
x_T = x_tp1;
Sigma_T = Sigma_t;
}
int initVars(Variable[] vars)
{
X = new Function[T][];
U = new Function[T - 1][];
int idx = 0;
for (int t = 0; t < T; ++t)
{
X[t] = new Function[XDIM];
for (int i = 0; i < XDIM; ++i)
{
X[t][i] = vars[idx++];
}
}
for (int t = 0; t < (T - 1); ++t)
{
U[t] = new Function[UDIM];
for (int i = 0; i < UDIM; ++i)
{
U[t][i] = vars[idx++];
}
}
q = new Function[QDIM];
for (int i = 0; i < QDIM; ++i)
{
q[i] = vars[idx++];
}
r = new Function[RDIM];
for (int i = 0; i < RDIM; ++i)
{
r[i] = vars[idx++];
}
Sigma_0 = new Function[XDIM, XDIM];
for (int i = 0; i < XDIM; ++i)
{
for (int j = 0; j < XDIM; ++j)
{
Sigma_0[i, j] = vars[idx++];
}
}
return(idx);
}
void EKF(Function[] x_t, Function[] u_t, Function[] q_t, Function[] r_t, Function[,] Sigma_t, out Function[] x_tp1, out Function[,] Sigma_tp1)
{
VM.checkSize <Function>(Sigma_t, XDIM, XDIM);
//Console.WriteLine("computeDynJacobians");
Function[,] A, M;
//computeDynJacobians(x_t, u_t, q_t, out A, out M);
A = VM.identity <Function>(XDIM, 0, 1); // d(dynfunc)/dx
M = VM.mult(VM.identity <Function>(UDIM, 0, 1), DT); // d(dynfunc)/dm (noise)
//Console.WriteLine("compute Sigma_tp1");
//VM.print<Function>(A);
//VM.print<Function>(Sigma_t);
//VM.print<Function>(M);
//VM.print<Function>(Q);
//Console.WriteLine("end of prints");
Sigma_tp1 = VM.plus(VM.mult(VM.mult(A, Sigma_t), VM.transpose <Function>(A)), VM.mult(VM.mult(M, Q), VM.transpose <Function>(M)));
//Console.WriteLine("after Sigma_tp1");
//VM.print<Function>(Sigma_tp1);
//VM.print<Function>(q_t);
for (int i = 0; i < QDIM; ++i)
{
q_t[i] = 0;
}
//Console.WriteLine("compute x_tp1");
x_tp1 = dynfunc(x_t, u_t, q_t);
//VM.print<Function>(x_tp1);
//Console.WriteLine("computeObsJacobians");
Function[,] H, N;
computeObsJacobians(x_tp1, r_t, out H, out N);
//Console.WriteLine("compute K1, K2, K");
Function[,] K1 = VM.mult(Sigma_tp1, VM.transpose <Function>(H));
Function[,] K2 = VM.plus(VM.mult(VM.mult(H, Sigma_tp1), VM.transpose <Function>(H)), VM.mult(VM.mult(N, R), VM.transpose <Function>(N)));
Function[,] K = VM.transpose(mdivide(VM.transpose(K2), VM.transpose(K1)));
//Console.WriteLine("compute Sigma_tp1");
Sigma_tp1 = VM.mult(VM.minus(VM.identity <Function>(XDIM, 0, 1), VM.mult(K, H)), Sigma_tp1);
//VM.print<Function>(Sigma_tp1);
}
Function costfunc(Function[][] X, Function[][] U, Function[] q, Function[] r, Function[,] Sigma_0, Function alpha_belief, Function alpha_control, Function alpha_final_belief)
{
int T = X.GetLength(0);
Function cost = Constant.newConstant(0.0);
Function[] x_tp1 = X[0];
Function[,] Sigma_t = Sigma_0, Sigma_tp1;
for (int t = 0; t < T - 1; ++t)
{
cost = cost + alpha_belief * VM.trace(Sigma_t);
cost = cost + alpha_control * VM.dot(U[t], U[t]);
EKF(X[t], U[t], q, r, Sigma_t, out x_tp1, out Sigma_tp1);
Sigma_t = Sigma_tp1;
}
cost = cost + alpha_final_belief * VM.trace(Sigma_t);
return(cost);
}
Function controlCostFunc(Function[][] U, Function[] q, Function[] r, Function[] x_0, Function[] x_goal, Function[,] Sigma_0, Function alpha_belief, Function alpha_control, Function alpha_final_belief, Function alpha_goal_state)
{
int T = U.GetLength(0) + 1;
Function cost = Constant.newConstant(0.0);
Function[] x_t = x_0, x_tp1;
Function[,] Sigma_t = Sigma_0, Sigma_tp1;
for (int t = 0; t < T - 1; ++t)
{
cost = cost + alpha_belief * VM.trace(Sigma_t);
cost = cost + alpha_control * VM.dot(U[t], U[t]);
EKF(x_t, U[t], q, r, Sigma_t, out x_tp1, out Sigma_tp1);
x_t = x_tp1;
Sigma_t = Sigma_tp1;
}
cost = cost + alpha_final_belief * VM.trace(Sigma_t) + alpha_goal_state * VM.dot(VM.minus(x_t, x_goal), VM.minus(x_t, x_goal));
return(cost);
}
void computeObsJacobians(Function[] x_tp1, Function[] r_t, out Function[,] H, out Function[,] N)
{
// H = d(obsfunc)/dx
H = new Function[ZDIM, XDIM];
for (int i = 0; i < XDIM; ++i)
{
Function[] Hicol = Function.D(obsfunc(x_tp1, r_t), x_tp1[i]);
for (int j = 0; j < ZDIM; ++j)
{
H[j, i] = Hicol[j];
}
}
// N = d(obsfunc)/dr
N = new Function[ZDIM, RDIM];
for (int i = 0; i < RDIM; ++i)
{
Function[] Nicol = Function.D(obsfunc(x_tp1, r_t), r_t[i]);
for (int j = 0; j < ZDIM; ++j)
{
N[j, i] = Nicol[j];
}
}
}
void computeDynJacobians(Function[] x_t, Function[] u_t, Function[] q_t, out Function[,] A, out Function[,] M)
{
// A = d(dynfunc)/dx
A = new Function[XDIM, XDIM];
for (int i = 0; i < XDIM; ++i)
{
Function[] Aicol = Function.D(dynfunc(x_t, u_t, q_t), x_t[i]);
for (int j = 0; j < XDIM; ++j)
{
A[i, j] = Aicol[j];
}
}
// M = d(dynfunc)/dq
M = new Function[XDIM, QDIM];
for (int i = 0; i < XDIM; ++i)
{
Function[] Micol = Function.D(dynfunc(x_t, u_t, q_t), q_t[i]);
for (int j = 0; j < QDIM; ++j)
{
M[i, j] = Micol[j];
}
}
}
void computeControlCostGradDiagHess()
{
// num timesteps
T = 40;
// variable instantiations
int nparams = 4;
int nvars = (T - 1) * UDIM + QDIM + RDIM + XDIM + XDIM + (XDIM * XDIM) + nparams;
Variable[] vars = new Variable[nvars];
for (int i = 0; i < nvars; ++i)
{
vars[i] = new Variable("vars_" + i);
}
U = new Function[T - 1][];
int idx = 0;
for (int t = 0; t < (T - 1); ++t)
{
U[t] = new Function[UDIM];
for (int i = 0; i < UDIM; ++i)
{
U[t][i] = vars[idx++];
}
}
q = new Function[] { vars[idx++], vars[idx++] };
r = new Function[] { vars[idx++], vars[idx++] };
Function[] x_0 = new Function[XDIM];
for (int i = 0; i < XDIM; ++i)
{
x_0[i] = vars[idx++];
}
Function[] x_goal = new Function[XDIM];
for (int i = 0; i < XDIM; ++i)
{
x_goal[i] = vars[idx++];
}
Sigma_0 = new Function[XDIM, XDIM];
for (int i = 0; i < XDIM; ++i)
{
for (int j = 0; j < XDIM; ++j)
{
Sigma_0[i, j] = vars[idx++];
}
}
Function alpha_belief = vars[idx++];
Function alpha_control = vars[idx++];
Function alpha_final_belief = vars[idx++];
Function alpha_goal_state = vars[idx++];
Function[] Uvec = VM.jaggedToLinear <Function>(U);
Function cost = controlCostFunc(U, q, r, x_0, x_goal, Sigma_0, alpha_belief, alpha_control, alpha_final_belief, alpha_goal_state);
Function[] costJacDiagHessFunc = new Function[2 * Uvec.Length + 1];
//Function[] costJacDiagHessFunc = new Function[1];
costJacDiagHessFunc[0] = cost;
idx = 1;
for (int i = 0; i < Uvec.Length; ++i)
{
costJacDiagHessFunc[idx++] = Function.D(cost, Uvec[i]);
}
for (int i = 0; i < Uvec.Length; ++i)
{
costJacDiagHessFunc[idx++] = Function.D(cost, Uvec[i], Uvec[i]);
}
Function costJacDiagHess = Function.derivative(costJacDiagHessFunc);
costJacDiagHess.printOperatorCounts();
bool[] inputVarIndices;
Variable[] costJacDiagHessVars = initializeInputVariables(costJacDiagHess, vars, out inputVarIndices);
costJacDiagHess.orderVariablesInDomain(costJacDiagHessVars);
costJacDiagHess.compileCCodeToFile("costControlJacDiagHess40.c");
//costJacDiagHess.compileCCodeToFile("costControl25.c");
System.IO.StreamWriter fh = new System.IO.StreamWriter("control-masks-40.txt");
fh.Write(nvars + " ");
for (int i = 0; i < nvars; ++i)
{
Console.WriteLine(inputVarIndices[i]);
if (inputVarIndices[i])
{
fh.Write("1 ");
}
else
{
fh.Write("0 ");
}
}
fh.WriteLine();
fh.Close();
}
void EKF(Function[] x_t, Function[,] Sigma_t, Function[] u_t, Function[] q_t, Function[] r_t, out Function[] x_tp1, out Function[,] Sigma_tp1)
{
VM.checkSize <Function>(Sigma_t, XDIM, XDIM);
Function[,] A = new Function[XDIM, XDIM];
for (int i = 0; i < XDIM; ++i)
{
Function[] Aicol = Function.D(dynfunc(x_t, u_t, q_t), x_t[i]);
for (int j = 0; j < XDIM; ++j)
{
A[i, j] = Aicol[j];
}
}
//Function[,] Ad = Function.derivative(A);
//VM.print<Function>(Ad);
Function[,] M = new Function[XDIM, QDIM];
for (int i = 0; i < XDIM; ++i)
{
Function[] Micol = Function.D(dynfunc(x_t, u_t, q_t), q_t[i]);
for (int j = 0; j < QDIM; ++j)
{
M[i, j] = Micol[j];
}
}
//VM.print<Function>(M);
//Function[,] Md = Function.derivative(M);
//VM.print<Function>(Md);
Sigma_tp1 = VM.plus(VM.mult(VM.mult(A, Sigma_t), VM.transpose <Function>(A)), VM.mult(VM.mult(M, Q), VM.transpose <Function>(M)));
//VM.print<Function>(Sigma_tp1);
for (int i = 0; i < QDIM; ++i)
{
q_t[i] = 0;
}
x_tp1 = dynfunc(x_t, u_t, q_t);
//VM.print<Function>(x_tp1);
//Console.WriteLine();
Function[,] H = new Function[ZDIM, XDIM];
for (int i = 0; i < ZDIM; ++i)
{
Function[] Hicol = Function.D(obsfunc(x_tp1, r_t), x_tp1[i]);
for (int j = 0; j < XDIM; ++j)
{
H[i, j] = Hicol[j];
}
}
//VM.print<Function>(H);
//Function[,] Hd = Function.derivative(H);
//VM.print<Function>(Hd);
Function[,] N = new Function[ZDIM, RDIM];
for (int i = 0; i < ZDIM; ++i)
{
Function[] Nicol = Function.D(obsfunc(x_tp1, r_t), r_t[i]);
for (int j = 0; j < RDIM; ++j)
{
N[i, j] = Nicol[j];
}
}
//VM.print<Function>(N);
//Function[,] Nd = Function.derivative(N);
//VM.print<Function>(Nd);
Function[,] K1 = VM.mult(Sigma_tp1, VM.transpose <Function>(H));
Function[,] K2 = VM.plus(VM.mult(VM.mult(H, Sigma_tp1), VM.transpose <Function>(H)), VM.mult(VM.mult(N, R), VM.transpose <Function>(N)));
Function[,] K = VM.transpose(mdivide(VM.transpose(K2), VM.transpose(K1)));
Function[,] I = VM.identity <Function>(XDIM, 0, 1);
Sigma_tp1 = VM.mult(VM.minus(I, VM.mult(K, H)), Sigma_tp1);
//VM.print<Function>(Sigma_tp1);
}
