// finds an estimate on the robots position gradient based on the sonar readings
public static double[] getGradient(LMMobileRobot thecart, int curOrientation, int prevOrientation)
{
double[] gradient = new double[] { 0, 0 };
double D1x, D2x, D3x, D4x, D5x, D6x;
double D1y, D2y, D3y, D4y, D5y, D6y;
double[] Snew = new double[8];
double[] Sprev = new double[8];
int i;
// storing sensor index permutations for both new and previous orientations
int[] prevPerm = getSensorPermutation(prevOrientation);
int[] newPerm = getSensorPermutation(curOrientation);
for (i = 0; i < 8; i++)
{
Snew[i] = (double)thecart.SonarArray[newPerm[i]];
Sprev[i] = (double)thecart.PrevSonarArray[prevPerm[i]];
}
// initially applying a Kalman Filter to the measurements
// a zero-input vector for the process
double[] U = new double[] { 0, 0, 0, 0, 0, 0, 0, 0 };
double[] B = new double[] { 0, 0, 0, 0, 0, 0, 0, 0 };
double[] W0 = new double[] { 0, 0, 0, 0, 0, 0, 0, 0 };
double[] V0 = new double[] { 2, 2, 2, 2, 2, 2, 2, 2 };
double[][] A = new double[][] { new double[] { 1, 0, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 1, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 1, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 1, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 1, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 1, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 1, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 0, 1 }
};
double[][] H = new double[][] { new double[] { 1, 0, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 1, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 1, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 1, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 1, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 1, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 1, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 0, 1 }
};
double[][] Q = new double[][] { new double[] { 5, 0, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 5, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 5, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 5, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 5, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 5, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 5, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 0, 5 }
};
double[][] R = new double[][] { new double[] { 2, 0, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 2, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 2, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 2, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 2, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 2, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 2, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 0, 2 }
};
KalmanFilter filter = new KalmanFilter(8, A, B, W0, H, V0, Q, R, Sprev);
// retrieving the measurement noise-free estimate by the Kalman filter,
// in order to use it to calculate the gradient
double[] Snest = filter.getNewEstimate(Snew, U);
// computing Gx
D1x = Sprev[1] * Math.Cos(Math.PI / 4) - Snest[1] * Math.Cos(Math.PI / 4);
//if (Math.Abs(D1x) > sup) D1x = 0;
D2x = Sprev[2] - Snest[2];
//if (Math.Abs(D2x) > sup) D2x = 0;
D3x = Sprev[3] * Math.Cos(Math.PI / 4) - Snest[3] * Math.Cos(Math.PI / 4);
//if (Math.Abs(D3x) > sup) D3x = 0;
D4x = -(Sprev[5] * Math.Cos(Math.PI / 4) - Snest[5] * Math.Cos(Math.PI / 4));
//if (Math.Abs(D4x) > sup) D4x = 0;
D5x = -(Sprev[6] - Snest[6]);
//if (Math.Abs(D5x) > sup) D5x = 0;
D6x = -(Sprev[7] * Math.Cos(Math.PI / 4) - Snest[7] * Math.Cos(Math.PI / 4));
//if (Math.Abs(D6x) > sup) D6x = 0;
gradient[0] = (D1x + D2x + D3x + D4x + D5x + D6x) / 6;
// computing Gy
D1y = Sprev[0] - Snest[0];
//if (Math.Abs(D1y) > sup) D1y = 0;
D2y = Sprev[1] * Math.Cos(Math.PI / 4) - Snest[1] * Math.Cos(Math.PI / 4);
//if (Math.Abs(D2y) > sup) D2y = 0;
D3y = -(Sprev[3] * Math.Cos(Math.PI / 4) - Snest[3] * Math.Cos(Math.PI / 4));
//if (Math.Abs(D3y) > sup) D3y = 0;
D4y = -(Sprev[4] - Snest[4]);
//if (Math.Abs(D4y) > sup) D4y = 0;
D5y = -(Sprev[5] * Math.Cos(Math.PI / 4) - Snest[5] * Math.Cos(Math.PI / 4));
//if (Math.Abs(D5y) > sup) D5y = 0;
D6y = Sprev[7] * Math.Cos(Math.PI / 4) - Snest[7] * Math.Cos(Math.PI / 4);
//if (Math.Abs(D6y) > sup) D6y = 0;
gradient[1] = (D1y + D2y + D3y + D4y + D5y + D6y) / 6;
return gradient;
}
private void button135_Click(object sender, EventArgs e)
{
double[][] A = new double[][] {new double[] {1, 0},
new double[] {0, 1}
};
double[] B = new double[] { 0, 0};
double[] V0 = new double[] { 2, 2};
double[] W0 = new double[] { 0, 0};
double[][] H = new double[][] {new double[] {1, 0},
new double[] {0, 1}
};
double[][] Q = new double[][] { new double[] {5, 0},
new double[] {0, 5}
};
double[][] R = new double[][] { new double[] {2, 0},
new double[] {0, 2}
};
double[] Z = new double[] {5, 5};
double[] Z1 = new double[] {6, 6};
double[] U = new double[] {0, 0};
KalmanFilter kalman = new KalmanFilter(2, A, B, W0, H, V0, Q, R, Z);
double[] est = kalman.getNewEstimate(Z1, U);
double[] Z2 = new double[] { 5, 5 };
kalman.getNewEstimate(Z2, U);
double[][] matrix = new double[][] { new double[] {1, 0, 0},
new double[] {2, 1, 0},
new double[] {1, 0, 1}};
double d = KalmanFilter.getDeterminant(matrix, 3);
double[][] inv = KalmanFilter.inverseMatrix(matrix, 3);
double[] Gxy = MappingFSM.getGradient(cart, MappingFSM.orEAST, MappingFSM.orNORTH);
double acos = MappingFSM.getAngleOffset(cart, MappingFSM.orEAST, MappingFSM.orNORTH);
double angle = 180*Math.Acos(acos)/Math.PI;
int test = 1;
}
// finds an estimate of the angular offset of the robot (unsigned)
// after a 90 turn
public static double getAngleOffset(LMMobileRobot thecart, int curOrientation, int prevOrientation)
{
double[] Snew = new double[8];
double[] Sprev = new double[8];
int i;
// storing sensor index permutations for both new and previous orientations
int[] prevPerm = getSensorPermutation(prevOrientation);
int[] newPerm = getSensorPermutation(curOrientation);
for (i = 0; i < 8; i++)
{
Snew[i] = (double)thecart.SonarArray[newPerm[i]];
Sprev[i] = (double)thecart.PrevSonarArray[prevPerm[i]];
}
// initially applying a Kalman Filter to the measurements
// a zero-input vector for the process
double[] U = new double[] { 0, 0, 0, 0, 0, 0, 0, 0 };
double[] B = new double[] { 0, 0, 0, 0, 0, 0, 0, 0 };
double[] W0 = new double[] { 0, 0, 0, 0, 0, 0, 0, 0 };
double[] V0 = new double[] { 2, 2, 2, 2, 2, 2, 2, 2 };
double[][] A = new double[][] { new double[] { 1, 0, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 1, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 1, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 1, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 1, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 1, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 1, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 0, 1 }
};
double[][] H = new double[][] { new double[] { 1, 0, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 1, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 1, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 1, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 1, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 1, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 1, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 0, 1 }
};
double[][] Q = new double[][] { new double[] { 5, 0, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 5, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 5, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 5, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 5, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 5, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 5, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 0, 5 }
};
double[][] R = new double[][] { new double[] { 2, 0, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 2, 0, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 2, 0, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 2, 0, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 2, 0, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 2, 0, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 2, 0 },
new double[] { 0, 0, 0, 0, 0, 0, 0, 2 }
};
KalmanFilter filter = new KalmanFilter(8, A, B, W0, H, V0, Q, R, Sprev);
// retrieving the measurement noise-free estimate by the Kalman filter,
// in order to use it to calculate the gradient
double[] Snest = filter.getNewEstimate(Snew, U);
// now computing the average cosine of the triangles
double anglesum = 0;
int count = 0;
for (i=0; i<8; i++)
if (Snest[i] < Snew[i])
{
count++;
anglesum += 180*Math.Acos((Snest[i]+11) / (Snew[i]+11))/Math.PI;
}
double average = (count == 0) ? 1 : anglesum / count;
return average;
}