public override ulong Create(BitmapData image)
{
var data = new DenseMatrix(32, 32);
using (var unmanaged = UnmanagedImage.FromManagedImage(image))
using (var filtered = ExtractChannel.Apply(unmanaged))
{
new Convolution(Filter, 9).ApplyInPlace(filtered);
using (var imgdata = new ResizeNearestNeighbor(32, 32).Apply(filtered))
{
unsafe
{
byte* src = (byte*)imgdata.ImageData.ToPointer();
int offset = imgdata.Stride - imgdata.Width;
int width = imgdata.Width;
int height = imgdata.Height;
for (int y = 0; y < height; y++)
{
for (int x = 0; x < width; x++, src++)
{
data.At(y, x, (float)*src / 255);
}
src += offset;
}
}
}
}
var dct = DctMatrix.FastDCT(data);
int dctsize = 8;
var vals = new List<double>();
for (int r = 1; r <= dctsize; r++)
{
for (int c = 1; c <= dctsize; c++)
{
vals.Add(dct[r, c]);
}
}
var sorted = new List<double>(vals);
sorted.Sort();
var mid = dctsize * dctsize / 2;
double median = (sorted[mid - 1] + sorted[mid]) / 2d;
ulong index = 1;
ulong result = 0;
for (int i = 0; i < dctsize * dctsize; i++)
{
if (vals[i] > median)
{
result |= index;
}
index = index << 1;
}
return result;
}
public void NormalizeMatrixTest()
{
//
Matrix matrix = new DenseMatrix(3, 3, -15);
Random random = new Random(2);
foreach (var elem in matrix.IndexedEnumerator())
{
matrix.At(elem.Item1, elem.Item2, elem.Item3 + 5 * random.NextDouble());
}
double min = 1.0, max = 2.5;
//
var normalizedMatrix = matrix.Normalize(max, min);
//
foreach (var row in normalizedMatrix.RowEnumerator())
{
Assert.IsTrue(row.Item2.Minimum() >= min);
Assert.IsTrue(row.Item2.Maximum() <= max);
}
}
/// <summary>
/// Compute the inverse of the upper triangle (in place).
/// </summary>
public static void InvertUpperTriangle(this DenseMatrix matrix)
{
int n = matrix.RowCount;
double sum;
for (int j = n - 1; j > -1; j--)
{
matrix.At(j, j, 1.0 / matrix.At(j, j));
for (int i = j - 1; i > -1; i--)
{
sum = 0.0;
for (int k = j; k > i; k--)
{
sum -= matrix.At(i, k) * matrix.At(k, j);
}
matrix.At(i, j, sum / matrix.At(i, i));
}
}
}
/// <summary>
/// Compute the inverse of the lower triangle (in place).
/// </summary>
public static void InvertLowerTriangle(this DenseMatrix matrix)
{
int n = matrix.RowCount;
double sum;
for (int j = 0; j < n; j++)
{
matrix.At(j, j, 1.0 / matrix.At(j, j));
for (int i = j + 1; i < n; i++)
{
sum = 0.0;
for (int k = j; k < i; k++)
{
sum -= matrix.At(i, k) * matrix.At(k, j);
}
matrix.At(i, j, sum / matrix.At(i, i));
}
}
}
/// <summary>
/// Run example
/// </summary>
public void Run()
{
// Format vector output to console
var formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone();
formatProvider.TextInfo.ListSeparator = " ";
// Create new empty square matrix
var matrix = new DenseMatrix(10);
Console.WriteLine(@"Empty matrix");
Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 1. Fill matrix by data using indexer []
var k = 0;
for (var i = 0; i < matrix.RowCount; i++)
{
for (var j = 0; j < matrix.ColumnCount; j++)
{
matrix[i, j] = k++;
}
}
Console.WriteLine(@"1. Fill matrix by data using indexer []");
Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 2. Fill matrix by data using At. The element is set without range checking.
for (var i = 0; i < matrix.RowCount; i++)
{
for (var j = 0; j < matrix.ColumnCount; j++)
{
matrix.At(i, j, k--);
}
}
Console.WriteLine(@"2. Fill matrix by data using At");
Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 3. Clone matrix
var clone = matrix.Clone();
Console.WriteLine(@"3. Clone matrix");
Console.WriteLine(clone.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 4. Clear matrix
clone.Clear();
Console.WriteLine(@"4. Clear matrix");
Console.WriteLine(clone.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 5. Copy matrix into another matrix
matrix.CopyTo(clone);
Console.WriteLine(@"5. Copy matrix into another matrix");
Console.WriteLine(clone.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 6. Get submatrix into another matrix
var submatrix = matrix.SubMatrix(2, 2, 3, 3);
Console.WriteLine(@"6. Copy submatrix into another matrix");
Console.WriteLine(submatrix.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 7. Get part of the row as vector. In this example: get 4 elements from row 5 starting from column 3
var row = matrix.Row(5, 3, 4);
Console.WriteLine(@"7. Get part of the row as vector");
Console.WriteLine(row.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 8. Get part of the column as vector. In this example: get 3 elements from column 2 starting from row 6
var column = matrix.Column(2, 6, 3);
Console.WriteLine(@"8. Get part of the column as vector");
Console.WriteLine(column.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 9. Get columns using column enumerator. If you need all columns you may use ColumnEnumerator without parameters
Console.WriteLine(@"9. Get columns using column enumerator");
foreach (var keyValuePair in matrix.ColumnEnumerator(2, 4))
{
Console.WriteLine(@"Column {0}: {1}", keyValuePair.Item1, keyValuePair.Item2.ToString("#0.00\t", formatProvider));
}
Console.WriteLine();
// 10. Get rows using row enumerator. If you need all rows you may use RowEnumerator without parameters
Console.WriteLine(@"10. Get rows using row enumerator");
foreach (var keyValuePair in matrix.RowEnumerator(4, 3))
{
Console.WriteLine(@"Row {0}: {1}", keyValuePair.Item1, keyValuePair.Item2.ToString("#0.00\t", formatProvider));
}
Console.WriteLine();
// 11. Convert matrix into multidimensional array
var data = matrix.ToArray();
Console.WriteLine(@"11. Convert matrix into multidimensional array");
for (var i = 0; i < data.GetLongLength(0); i++)
{
for (var j = 0; j < data.GetLongLength(1); j++)
{
Console.Write(data[i, j].ToString("#0.00\t"));
}
Console.WriteLine();
}
Console.WriteLine();
// 12. Convert matrix into row-wise array
var rowwise = matrix.ToRowWiseArray();
Console.WriteLine(@"12. Convert matrix into row-wise array");
for (var i = 0; i < matrix.RowCount; i++)
{
for (var j = 0; j < matrix.ColumnCount; j++)
{
Console.Write(rowwise[(i * matrix.ColumnCount) + j].ToString("#0.00\t"));
}
Console.WriteLine();
}
Console.WriteLine();
// 13. Convert matrix into column-wise array
var columnise = matrix.ToColumnWiseArray();
Console.WriteLine(@"13. Convert matrix into column-wise array");
for (var i = 0; i < matrix.RowCount; i++)
{
for (var j = 0; j < matrix.ColumnCount; j++)
{
Console.Write(columnise[(j * matrix.RowCount) + i].ToString("#0.00\t"));
}
Console.WriteLine();
}
Console.WriteLine();
// 14. Get matrix diagonal as vector
var diagonal = matrix.Diagonal();
Console.WriteLine(@"14. Get matrix diagonal as vector");
Console.WriteLine(diagonal.ToString("#0.00\t", formatProvider));
Console.WriteLine();
}
/// <summary>
/// Create a matrix based on this vector in row form (one single row).
/// </summary>
/// <returns>This vector as a row matrix.</returns>
public override Matrix<double> ToRowMatrix()
{
var matrix = new DenseMatrix(1, _length);
for (var i = 0; i < _values.Length; i++)
{
matrix.At(0, i, _values[i]);
}
return matrix;
}
public static Matrix quat_to_DCM(Matrix q)
{
Matrix DCM = new DenseMatrix(3, 3, 0);
q = quat_norm(q);
DCM.At(0, 0, (q.At(0, 0) * q.At(0, 0) + q.At(0, 1) * q.At(0, 1) - q.At(0, 2) * q.At(0, 2) - q.At(0, 3) * q.At(0, 3)));
DCM.At(0, 1, 2 * (q.At(0, 1) * q.At(0, 2) + q.At(0, 0) * q.At(0, 3)));
DCM.At(0, 2, 2 * (q.At(0, 1) * q.At(0, 3) - q.At(0, 0) * q.At(0, 2)));
DCM.At(1, 0, 2 * (q.At(0, 1) * q.At(0, 2) - q.At(0, 0) * q.At(0, 3)));
DCM.At(1, 1, (q.At(0, 0) * q.At(0, 0) - q.At(0, 1) * q.At(0, 1) + q.At(0, 2) * q.At(0, 2) - q.At(0, 3) * q.At(0, 3)));
DCM.At(1, 2, 2 * (q.At(0, 2) * q.At(0, 3) + q.At(0, 0) * q.At(0, 1)));
DCM.At(2, 0, 2 * (q.At(0, 1) * q.At(0, 3) + q.At(0, 0) * q.At(0, 2)));
DCM.At(2, 1, 2 * (q.At(0, 2) * q.At(0, 3) - q.At(0, 0) * q.At(0, 1)));
DCM.At(2, 2, (q.At(0, 0) * q.At(0, 0) - q.At(0, 1) * q.At(0, 1) - q.At(0, 2) * q.At(0, 2) + q.At(0, 3) * q.At(0, 3)));
return DCM;
}
public static Matrix mrotate(Matrix q, Matrix w, double dT)
{
double Fx = w.At(0, 0) * dT;
double Fy = w.At(1, 0) * dT;
double Fz = w.At(2, 0) * dT;
double Fm = (double)Math.Sqrt(Fx * Fx + Fy * Fy + Fz * Fz);
double sinFm2 = (double)Math.Sin(Fm / 2);
double cosFm2 = (double)Math.Cos(Fm / 2);
Matrix Nd = new DenseMatrix(1, 4, 0);
if (Fm != (double)0)
{
Nd.At(0, 0, cosFm2);
Nd.At(0, 1, (Fx / Fm) * sinFm2);
Nd.At(0, 2, (Fy / Fm) * sinFm2);
Nd.At(0, 3, (Fz / Fm) * sinFm2);
}
else Nd.At(0, 0, 1);
q = quat_multi(q, Nd);
return q;
}
public static Matrix Matrix_Plus(Matrix A, Matrix B)
{
Matrix C = new DenseMatrix(A.RowCount, A.ColumnCount, 0);
int i, j;
if (A.RowCount != B.RowCount | A.ColumnCount != B.ColumnCount) return C;
for (i = 0; i < A.RowCount; i++)
{
for (j = 0; j < A.ColumnCount; j++)
{
C.At(i, j, A.At(i, j) + B.At(i, j));
}
}
return C;
}
public static Matrix Matrix_Const_Mult(Matrix A, double B)
{
Matrix C = new DenseMatrix(A.RowCount, A.ColumnCount, 0);
int i, j;
for (i = 0; i < A.RowCount; i++)
{
for (j = 0; j < A.ColumnCount; j++)
{
C.At(i, j, A.At(i, j)*B);
}
}
return C;
}
public Matrix<double> Downsample_Skippixel(Matrix<double> baseImage)
{
int rows2 = baseImage.RowCount / 2;
int cols2 = baseImage.ColumnCount / 2;
var scaledImage = new DenseMatrix(rows2, cols2);
// Save each second pixel
for(int c = 0; c < scaledImage.ColumnCount; ++c)
{
for(int r = 0; r < scaledImage.RowCount; ++r)
{
scaledImage.At(r, c,
baseImage.At(2 * r, 2 * c));
}
}
return scaledImage;
}
public Matrix<double> Downsample_Gauss33(Matrix<double> baseImage)
{
int rows2 = baseImage.RowCount / 2;
int cols2 = baseImage.ColumnCount / 2;
var scaledImage = new DenseMatrix(rows2, cols2);
double sum;
int lr = scaledImage.RowCount - 1;
int lc = scaledImage.ColumnCount - 1;
// TODO: borders
for(int c = 0; c < lc; ++c)
{
for(int r = 0; r < lr; ++r)
{
sum = baseImage.At(2 * r, 2 * c) * _gauss33[0] +
baseImage.At(2 * r, 2 * c + 1) * _gauss33[1] +
baseImage.At(2 * r, 2 * c + 2) * _gauss33[0] +
baseImage.At(2 * r + 1, 2 * c) * _gauss33[1] +
baseImage.At(2 * r + 1, 2 * c + 1) * _gauss33[2] +
baseImage.At(2 * r + 1, 2 * c + 2) * _gauss33[1] +
baseImage.At(2 * r + 2, 2 * c) * _gauss33[0] +
baseImage.At(2 * r + 2, 2 * c + 1) * _gauss33[1] +
baseImage.At(2 * r + 2, 2 * c + 2) * _gauss33[0];
scaledImage.At(r, c, sum);
}
// Last row -> mirror one out of bounds
sum = baseImage.At(2 * lr, 2 * c) * _gauss33[0] * 2.0 +
baseImage.At(2 * lr, 2 * c + 1) * _gauss33[1] * 2.0 +
baseImage.At(2 * lr, 2 * c + 2) * _gauss33[0] * 2.0 +
baseImage.At(2 * lr + 1, 2 * c) * _gauss33[1] +
baseImage.At(2 * lr + 1, 2 * c + 1) * _gauss33[2] +
baseImage.At(2 * lr + 1, 2 * c + 2) * _gauss33[1];
scaledImage.At(lr, c, sum);
}
// Last column -> mirror one out of bounds
for(int r = 0; r < lr; ++r)
{
sum = baseImage.At(2 * r, 2 * lc) * _gauss33[0] * 2.0 +
baseImage.At(2 * r, 2 * lc + 1) * _gauss33[1] +
baseImage.At(2 * r + 1, 2 * lc) * _gauss33[1] * 2.0 +
baseImage.At(2 * r + 1, 2 * lc + 1) * _gauss33[2] +
baseImage.At(2 * r + 2, 2 * lc) * _gauss33[0] * 2.0 +
baseImage.At(2 * r + 2, 2 * lc + 1) * _gauss33[1];
scaledImage.At(r, lc, sum);
}
// Last row -> mirror row/col out of bounds=
sum = baseImage.At(2 * lr, 2 * lc) * _gauss33[0] * 4.0 +
baseImage.At(2 * lr, 2 * lc + 1) * _gauss33[1] * 2.0 +
baseImage.At(2 * lr + 1, 2 * lc) * _gauss33[1] * 2.0 +
baseImage.At(2 * lr + 1, 2 * lc + 1) * _gauss33[2];
scaledImage.At(lr, lc, sum);
return scaledImage;
}
public Matrix<double> Downsample_Average22(Matrix<double> baseImage)
{
int rows2 = baseImage.RowCount / 2;
int cols2 = baseImage.ColumnCount / 2;
var scaledImage = new DenseMatrix(rows2, cols2);
// For each pixel set average of 2x2 neighbourhood
for(int c = 0; c < scaledImage.ColumnCount; ++c)
{
for(int r = 0; r < scaledImage.RowCount; ++r)
{
double sum = baseImage.At(2 * r, 2 * c) + baseImage.At(2 * r, 2 * c + 1) +
baseImage.At(2 * r + 1, 2 * c) + baseImage.At(2 * r + 1, 2 * c + 1);
scaledImage.At(r, c, 0.25 * sum);
}
}
return scaledImage;
}
private void write_data(packet[] source, int index)
{
int length = source.Length;
saveBox.Text = "Сохранение файла " + index;
saveBox.Update();
progressBar.Value = 0;
progressBar.Maximum = length;
string additional = "";
double[] magn_c = new double[12];
double[] accl_c = new double[12];
double[] gyro_c = new double[12];
double Mk = 1;
double Wk = 1;
double Ak = 1;
switch (index)
{
case 1:
additional = "left_oar";
double[] temp1 = { 0.021012275928952, 0.067860169975568, 0.000127689343889, 0.045296151352541,
-0.009324017077391, 0.026722923577867, 0.008307857594568, 0.005284287915262,
-0.035947192687517, -0.012542925115207, 0.061512131892943, 0.004409782721854 };
accl_c = temp1;
Ak = 0.996259867299029;
double[] temp11 = { 0.002873025761360, -0.000277783847693, -0.008949237789463, 0.024839518300883,
0.018134114269133, 0.004957678597933, 0.059903966032509, -0.020001702990621,
-0.048455202844697, 0.040007724599001, -0.002723436449578, 0.028657182612509 };
magn_c = temp11;
Mk = 1.989928230657113;
Wk = 1.024951581925949;
break;
case 2:
additional = "right_oar";
double[] temp2 = { 0.006489103530672, 0.007711972047671, 0.008910927008256, 0.004526628966860,
-0.030613701020120, 0.009733926042102, 0.000520434141698, -0.020438431372878,
-0.000690207812167, 0.003527827615877, 0.003268096379523, -0.009084146831565 };
accl_c = temp2;
Ak = 0.993917643789002;
double[] temp22 = {-0.004338472524034, -0.007279143920897, -0.016596561254121, -0.040100105586272,
0.026447190185438, 0.062723828297715, 0.014235202706375, -0.032778454891706,
-0.016686216543873, 0.049183804843751, -0.170661765769371, 0.174784618340082 };
magn_c = temp22;
Mk = 1.981050375613622;
Wk = 0.999473669119779;
break;
case 3:
additional = "first_hand";
double[] temp3 = { 0.012248789806271, 0.010963952889031, 0.010082940049261, 0.017732310046796,
-0.010807018986018, -0.017866987099261, -0.038976713533177, 0.011937965171044,
0.017418474194167, 0.010157002539499, 0.006639710879831, -0.008997250633174 };
accl_c = temp3;
Ak = 0.995116429313122;
double[] temp33 = {-0.024902579646778, -0.020478721707093, -0.026353375666180, -0.001744353714878,
0.001038721251414, -0.008552827405651, 0.000013148017514, 0.005334631470956,
-0.004628038571913, -0.041660570389140, 0.203013174006507, 0.110944181992475 };
magn_c = temp33;
Mk = 2.000804739331122;
Wk = 1.005957834380545;
break;
case 4:
additional = "second_hand";
double[] temp4 = { 0.009773445955991, 0.010515969102165, 0.046332523300049, -0.009734857220358,
0.088901575635824, 0.011068909952491, -0.012572850762404, 0.170713670954205,
-0.008974449587517, 0.001063872406408, -0.005104328004269, -0.003338723771119 };
accl_c = temp4;
Ak = 0.995096553223738;
double[] temp44 = { 0.014780841871083, 0.018746310904878, 0.009620181136691, -0.006116082059700,
-0.000324924134046, -0.004326570176434, -0.007611544691470, 0.009652319917883,
0.002813293660957, 0.093081170636918, 0.035125384985654, -0.014316950547061 };
magn_c = temp44;
Mk = 2.063752665877927;
Wk = 1.013532577606638;
break;
case 5:
additional = "seat";
double[] temp5 = { 0.018860976044349, 0.012994456079329, -0.017726498806178, 0.014155888603851,
0.135660705289298, -0.005517590513633, -0.006515740524433, 0.089222505526992,
0.007556195473340, -0.034224056011697, -0.032625814493799, -0.091725250270515 };
accl_c = temp5;
Ak = 0.990392687940513;
break;
}
FileStream fs_imu = File.Create(saveFileDialog.FileName + "_" + additional + ".imu", 2048, FileOptions.None);
BinaryWriter str_imu = new BinaryWriter(fs_imu);
Int16 buf16; Byte buf8; Int32 buf32;
Double bufD; Single bufS; UInt32 bufU32;
DenseVector Magn_coefs = new DenseVector(12);
DenseVector Accl_coefs = new DenseVector(12);
DenseVector Gyro_coefs = new DenseVector(12);
Kalman_class.Parameters Parameters = new Kalman_class.Parameters(Accl_coefs, Magn_coefs, Gyro_coefs);
Kalman_class.Sensors Sensors = new Kalman_class.Sensors(new DenseMatrix(1, 3, 0), new DenseMatrix(1, 3, 0), new DenseMatrix(1, 3, 0));
Matrix Initia_quat = new DenseMatrix(1, 4, 0);
Initia_quat.At(0, 0, 1);
Kalman_class.State State = new Kalman_class.State(Kalman_class.ACCLERATION_NOISE, Kalman_class.MAGNETIC_FIELD_NOISE, Kalman_class.ANGULAR_VELOCITY_NOISE,
Math.Pow(10, -6), Math.Pow(10, -15), Math.Pow(10, -15), Initia_quat);
double[] angles = new double[3];
double[] mw, ma, mm;
ma = new double[3];
mw = new double[3];
mm = new double[3];
Tuple<Vector, Kalman_class.Sensors, Kalman_class.State> AHRS_result;
double[] w_helper = new double[source.Length];
double[] anglex = new double[source.Length];
double[] angley = new double[source.Length];
double[] anglez = new double[source.Length];
//for (int i = 0; i < w_helper.Length; i++) w_helper[i] = source[i].w[0];
//anglex = Signal_processing.Zero_average_corr(w_helper, w_helper.Length);
//for (int i = 0; i < w_helper.Length; i++) w_helper[i] = source[i].w[1];
//angley = Signal_processing.Zero_average_corr(w_helper, w_helper.Length);
//for (int i = 0; i < w_helper.Length; i++) w_helper[i] = source[i].w[2];
//anglez = Signal_processing.Zero_average_corr(w_helper, w_helper.Length);
double[] read_coefs = new double[0];
double additional_mult = 1;
switch (source[0].type)
{
case 0x41:
double[] tempc1 = { 0.000833, 0.04 * Math.PI / 180F, 0.00014286, 0.00002, 31, 0.07386, 0, 0 };
read_coefs = tempc1;
break;
case 0x51:
double[] tempc2 = { 0.0039, 0, 0, 0, 0, 0, 0, 0 };
read_coefs = tempc2;
break;
case 0x61:
double[] tempc3 = { 0.0008, 0.02 * Math.PI / 180F, 0.0001, 0.00004, 25, 0.00565, 0.0055, 0.000030518 };
read_coefs = tempc3;
additional_mult = -1;
break;
}
double[] quats = new double[4];
DenseMatrix quat_m = new DenseMatrix(1, 4);
Matrix DCM = new DenseMatrix(3, 3);
Matrix sens = new DenseMatrix(1, 3);
Matrix res = new DenseMatrix(1, 3);
double tempr = new double();
double sqr = new double();
for (int i = 0; i < length; i++)
{
progressBar.Value++;
// умножены на -1 для того чтобы оси соответствовали правой тройке
// и осям на датчиках
mw[0] = source[i].w[0] * read_coefs[1]*Wk;// *-1;
mw[1] = source[i].w[1] * read_coefs[1]*Wk;// * -1;
mw[2] = source[i].w[2] * read_coefs[1]*Wk;// * -1;
ma[0] = source[i].a[0] * read_coefs[0]*Ak;// * -1;
ma[1] = source[i].a[1] * read_coefs[0]*Ak;// * -1;
ma[2] = source[i].a[2] * read_coefs[0]*Ak;// * -1;
mm[0] = source[i].m[0] * read_coefs[2]*Mk;// * -1;
mm[1] = source[i].m[1] * read_coefs[2]*Mk;// * -1;
mm[2] = source[i].m[2] * read_coefs[2]*Mk;// * -1;
//mw = single_correction(gyro_c, w[i, 0], w[i, 1], w[i, 2]);
Sensors.a.At(0, 0, ma[0]);
Sensors.a.At(0, 1, ma[1]);
Sensors.a.At(0, 2, ma[2]);
Sensors.w.At(0, 0, mw[0]);
Sensors.w.At(0, 1, mw[1]);
Sensors.w.At(0, 2, mw[2]);
Sensors.m.At(0, 0, mm[0]);
Sensors.m.At(0, 1, mm[1]);
Sensors.m.At(0, 2, mm[2]);
AHRS_result = Kalman_class.AHRS_LKF_EULER(Sensors, State, Parameters);
State = AHRS_result.Item3;
mm = single_correction(magn_c, mm[0], mm[1], mm[2]);
ma = single_correction(accl_c, ma[0], ma[1], ma[2]);
//------------------------------------------------------------------------
//mm = single_correction(magn_c, m[i, 0], m[i, 1], m[i, 2]);
//ma = single_correction(accl_c, a[i, 0], a[i, 1], a[i, 2]);
//mw = single_correction(gyro_c, w[i, 0], w[i, 1], w[i, 2]);
//----------------------------------------------------------------------
//----------------------------------------------------------------------
angles[0] = (AHRS_result.Item1.At(0));
angles[1] = (AHRS_result.Item1.At(1));
angles[2] = (AHRS_result.Item1.At(2));
//angles[0] = (anglez[i]);
//angles[1] = (angley[i]);
//angles[2] = (anglex[i]);
//sqr = Math.Sqrt(mm[0]*mm[0] + mm[1]*mm[1] + mm[2]*mm[2]);
//mm[0] = mm[0] / sqr;
//mm[1] = mm[1] / sqr;
//mm[2] = mm[2] / sqr;
//sqr = sqr;
//if (source[i].quat[1] != 0)
//{
// quats[0] = source[i].quat[0] * read_coefs[6];
// quats[1] = source[i].quat[1] * read_coefs[7];
// quats[2] = source[i].quat[2] * read_coefs[7];
// quats[3] = source[i].quat[3] * read_coefs[7];
// quat_m.At(0, 0, quats[0]);
// quat_m.At(0, 1, quats[1]);
// quat_m.At(0, 2, quats[2]);
// quat_m.At(0, 3, quats[3]);
// DCM = Kalman_class.quat_to_DCM(quat_m);
// res = Kalman_class.dcm2angle(DCM);
// //angles = quat2angle(quats);
// //DCM = Kalman_class.Matrix_Transpose(quat2dcm(quats));
// angles[0] = res.At(0, 0);
// angles[1] = res.At(0, 1);
// angles[2] = res.At(0, 2);
// //DMC = Kalman_class.Matrix_Transpose(DMC);
// /*sens.At(0, 0, ma[0]);
// sens.At(0, 1, ma[1]);
// sens.At(0, 2, ma[2]);
// res = Kalman_class.Matrix_Mult(sens, DCM);
// ma[0] = res.At(0, 0);
// ma[1] = res.At(0, 1);
// ma[2] = res.At(0, 2);
// sens.At(0, 0, mw[0]);
// sens.At(0, 1, mw[1]);
// sens.At(0, 2, mw[2]);
// res = Kalman_class.Matrix_Mult(sens, DCM);
// mw[0] = res.At(0, 0);
// mw[1] = res.At(0, 1);
// mw[2] = res.At(0, 2);
// sens.At(0, 0, mm[0]);
// sens.At(0, 1, mm[1]);
// sens.At(0, 2, mm[2]);
// res = Kalman_class.Matrix_Mult(sens, DCM);
// mm[0] = res.At(0, 0);
// mm[1] = res.At(0, 1);
// mm[2] = res.At(0, 2);*/
//}
tempr = read_coefs[4] + source[i].temper * read_coefs[5];
// IMU
// QQ
buf16 = (Int16)(angles[0] * 10000);
str_imu.Write(buf16);
buf16 = (Int16)(angles[1] * 10000);
str_imu.Write(buf16);
buf16 = (Int16)(angles[2] * 10000);
str_imu.Write(buf16);
// w
buf16 = (Int16)(mw[0] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(mw[1] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(mw[2] * 3000);
str_imu.Write(buf16);
// a
buf16 = (Int16)(ma[0] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(ma[1] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(ma[2] * 3000);
str_imu.Write(buf16);
// m
buf16 = (Int16)(mm[0] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(mm[1] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(mm[2] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(tempr); // t
str_imu.Write(buf16);
//buf32 = (Int32)(counter[i]);
buf32 = (Int32)(source[i].ticks); // pps_imu
str_imu.Write(buf32);
buf8 = (Byte)(0); // check_sum
str_imu.Write(buf8);
}
str_imu.Flush();
str_imu.Close();
}
private double[] single_correction(double[] coefs, double xdata, double ydata, double zdata)
{
double[] result = new double[3];
Matrix B = new DiagonalMatrix(3, 3, 1);
Matrix A = new DenseMatrix(3, 3);
A.At(0, 0, coefs[0]);
A.At(0, 1, coefs[3]);
A.At(0, 2, coefs[4]);
A.At(1, 0, coefs[5]);
A.At(1, 1, coefs[1]);
A.At(1, 2, coefs[6]);
A.At(2, 0, coefs[7]);
A.At(2, 1, coefs[8]);
A.At(2, 2, coefs[2]);
Matrix B1 = Kalman_class.Matrix_Minus(B, A);
Matrix C = new DenseMatrix(3, 1);
C.At(0, 0, xdata);
C.At(1, 0, ydata);
C.At(2, 0, zdata);
Matrix D = new DenseMatrix(3, 1);
D.At(0, 0, coefs[9]);
D.At(1, 0, coefs[10]);
D.At(2, 0, coefs[11]);
Matrix res = new DenseMatrix(3, 1);
res = Kalman_class.Matrix_Mult(B1, Kalman_class.Matrix_Minus(C, D));
result[0] = res.At(0, 0);
result[1] = res.At(1, 0);
result[2] = res.At(2, 0);
return result;
}
private Matrix quat2dcm(double[] quat)
{
Matrix result = new DenseMatrix(3, 3, 0);
result.At(0, 0, (1 - 2 * (quat[2] * quat[2] + quat[3] * quat[3])));
result.At(0, 1, 2 * (quat[1] * quat[2] - quat[3] * quat[0]));
result.At(0, 2, 2 * (quat[1] * quat[3] + quat[2] * quat[0]));
result.At(1, 0, 2 * (quat[1] * quat[2] + quat[3] * quat[0]));
result.At(1, 1, 1 - 2 * (quat[1] * quat[1] + quat[3] * quat[3]));
result.At(1, 2, 2 * (quat[2] * quat[3] - quat[1] * quat[0]));
result.At(2, 0, 2 * (quat[1] * quat[3] - quat[2] * quat[0]));
result.At(2, 1, 2 * (quat[2] * quat[3] + quat[1] * quat[0]));
result.At(2, 2, 1 - 2 * (quat[1] * quat[1] + quat[2] * quat[2]));
return result;
}
public static Matrix dcm2quat(Matrix DCM)
{
Matrix q = new DenseMatrix(1, 4, 0);
double tr = trace (DCM);
if (tr > 0)
{
double sqtrp1 = (double)Math.Sqrt(tr + 1);
q.At(0, 0, (double)0.5 * sqtrp1);
q.At(0, 1, (DCM.At(1, 2) - DCM.At(2, 1)) / (2 * sqtrp1));
q.At(0, 2, (DCM.At(2, 0) - DCM.At(0, 2)) / (2 * sqtrp1));
q.At(0, 3, (DCM.At(0, 1) - DCM.At(1, 0)) / (2 * sqtrp1));
return q;
}
else
{
Matrix d = new DenseMatrix(1,3,0);
d.At(0, 0, DCM.At(0, 0));
d.At(0, 1, DCM.At(1, 1));
d.At(0, 2, DCM.At(2, 2));
if ((d.At(0,1)>d.At(0,0)&(d.At(0,1)>d.At(0,2))))
{
double sqdip1 = (double)Math.Sqrt(d.At(0, 1) - d.At(0, 0) - d.At(0, 2) + 1);
q.At(0, 2, 0.5 * sqdip1);
if (sqdip1 != 0)
{
sqdip1 = 0.5 / sqdip1;
}
q.At(0, 0, (DCM.At(2, 0) - DCM.At(0, 2)) * sqdip1);
q.At(0, 1, (DCM.At(0, 1) + DCM.At(1, 0)) * sqdip1);
q.At(0, 3, (DCM.At(1, 2) + DCM.At(2, 1)) * sqdip1);
}
else if (d.At(0, 2) > d.At(0, 0))
{
double sqdip1 = (double)Math.Sqrt(d.At(0, 2) - d.At(0, 0) - d.At(0, 1) + 1);
q.At(0, 3, 0.5 * sqdip1);
if (sqdip1 != 0)
{
sqdip1 = 0.5 / sqdip1;
}
q.At(0, 0, (DCM.At(0, 1) - DCM.At(1, 0)) * sqdip1);
q.At(0, 1, (DCM.At(2, 0) + DCM.At(0, 2)) * sqdip1);
q.At(0, 2, (DCM.At(1, 2) + DCM.At(2, 1)) * sqdip1);
}
else
{
double sqdip1 = (double)Math.Sqrt(d.At(0, 0) - d.At(0, 1) - d.At(0, 2) + 1);
q.At(0, 1, 0.5 * sqdip1);
if (sqdip1 != 0)
{
sqdip1 = 0.5 * sqdip1;
}
q.At(0, 0, (DCM.At(1, 2) - DCM.At(2, 1)) * sqdip1);
q.At(0, 2, (DCM.At(0, 1) + DCM.At(1, 0)) * sqdip1);
q.At(0, 3, (DCM.At(2, 1) + DCM.At(1, 2)) * sqdip1);
}
}
return q;
}
public Matrix<double> Upsample_Copy(Matrix<double> baseImage)
{
int rows2 = baseImage.RowCount * 2;
int cols2 = baseImage.ColumnCount * 2;
var scaledImage = new DenseMatrix(rows2, cols2);
for(int c = 0; c < baseImage.ColumnCount; ++c)
{
for(int r = 0; r < baseImage.RowCount; ++r)
{
scaledImage.At(2 * r, 2 * c, baseImage.At(r, c));
scaledImage.At(2 * r + 1, 2 * c, baseImage.At(r, c));
scaledImage.At(2 * r, 2 * c + 1, baseImage.At(r, c));
scaledImage.At(2 * r + 1, 2 * c + 1, baseImage.At(r, c));
}
}
return scaledImage;
}
public static Matrix Matrix_Mult(Matrix A, Matrix B)
{
Matrix C = new DenseMatrix(A.RowCount, B.ColumnCount, 0);
int i, j, k;
double el;
if (A.ColumnCount != B.RowCount) return C;
for (i = 0; i < A.RowCount; i++)
{
for (j = 0; j < B.ColumnCount; j++)
{
el = 0;
for (k = 0; k < B.RowCount; k++)
{
el = el + A.At(i, k) * B.At(k, j);
}
C.At(i, j, el);
}
}
return C;
}
public Matrix<double> Upsample_CrossAverage(Matrix<double> baseImage)
{
int rows2 = baseImage.RowCount * 2;
int cols2 = baseImage.ColumnCount * 2;
double sum;
int lr = baseImage.RowCount - 1;
int lc = baseImage.ColumnCount - 1;
var scaledImage = new DenseMatrix(rows2, cols2);
// TODO consider borders
// 1) Base image pixels have (2r0,2c0) coords
// 2) For each (2r0+1,2c0+1) let it be average of 4 diagonal neighbours
// 3 For each (2r0,2c0+1),(2r0+2,2c0) let it be average of 4 edge neighbours (including thise from 2)
for(int c = 0; c < lc; ++c)
{
for(int r = 0; r < lr; ++r)
{
sum = baseImage.At(r, c) + baseImage.At(r + 1, c + 1)
+ baseImage.At(r, c + 1) + baseImage.At(r + 1, c);
scaledImage.At(2 * r, 2 * c, baseImage.At(r, c));
scaledImage.At(2 * r + 1, 2 * c + 1, 0.25 * sum);
}
sum = baseImage.At(lr, c) + baseImage.At(lr - 1, c + 1)
+ baseImage.At(lr, c + 1) + baseImage.At(lr - 1, c);
scaledImage.At(2 * lr, 2 * c, baseImage.At(lr, c));
scaledImage.At(2 * lr + 1, 2 * c + 1, 0.25 * sum);
scaledImage.At(lr, c, sum);
}
// Last column -> mirror one out of bounds
for(int r = 0; r < lr; ++r)
{
sum = baseImage.At(r, lc) + baseImage.At(r + 1, lc - 1)
+ baseImage.At(r, lc - 1) + baseImage.At(r + 1, lc);
scaledImage.At(2 * r, 2 * lc, baseImage.At(r, lc));
scaledImage.At(2 * r + 1, 2 * lc + 1, 0.25 * sum);
}
// Last row -> mirror row/col out of bounds
sum = baseImage.At(lr, lc) + baseImage.At(lr - 1, lc - 1)
+ baseImage.At(lr, lc - 1) + baseImage.At(lr - 1, lc);
scaledImage.At(2 * lr, 2 * lc, baseImage.At(lr, lc));
scaledImage.At(2 * lr + 1, 2 * lc + 1, 0.25 * sum);
// First row/column
for(int r = 0; r < lr; ++r)
{
sum = baseImage.At(r, 0) + baseImage.At(r + 1, 0) +
scaledImage.At(2 * r + 1, 1) * 2.0;
scaledImage.At(2 * r + 1, 0, 0.25 * sum);
}
sum = baseImage.At(lr, 0) + baseImage.At(lr - 1, 0) +
scaledImage.At(2 * lr + 1, 1) * 2.0;
scaledImage.At(2 * lr + 1, 0, 0.25 * sum);
for(int c = 0; c < lc; ++c)
{
sum = baseImage.At(0, c) + baseImage.At(0, c + 1) +
scaledImage.At(1, 2 * c + 1) * 2.0;
scaledImage.At(0, 2 * c + 1, 0.25 * sum);
}
sum = baseImage.At(0, lc) + baseImage.At(0, lc - 1) +
scaledImage.At(1, 2 * lc + 1) * 2.0;
scaledImage.At(0, 2 * lc + 1, 0.25 * sum);
for(int c = 0; c < lc; ++c)
{
for(int r = 0; r < lr; ++r)
{
sum = baseImage.At(r, c) + baseImage.At(r + 1, c) +
scaledImage.At(2 * r + 1, 2 * c + 1) + scaledImage.At(2 * r + 1, 2 * c - 1);
scaledImage.At(2 * r + 1, 2 * c, 0.25 * sum);
sum = baseImage.At(r, c) + baseImage.At(r, c + 1) +
scaledImage.At(2 * r + 1, 2 * c + 1) + scaledImage.At(2 * r - 1, 2 * c + 1);
scaledImage.At(2 * r, 2 * c + 1, 0.25 * sum);
}
sum = baseImage.At(lr, c) + baseImage.At(lr - 1, c) +
scaledImage.At(2 * lr + 1, 2 * c + 1) + scaledImage.At(2 * lr + 1, 2 * c - 1);
scaledImage.At(2 * lr + 1, 2 * c, 0.25 * sum);
sum = baseImage.At(lr, c) + baseImage.At(lr, c + 1) +
scaledImage.At(2 * lr + 1, 2 * c + 1) + scaledImage.At(2 * lr - 1, 2 * c + 1);
scaledImage.At(2 * lr, 2 * c + 1, 0.25 * sum);
}
for(int r = 0; r < lr; ++r)
{
sum = baseImage.At(r, lc) + baseImage.At(r + 1, lc) +
scaledImage.At(2 * r + 1, 2 * lc + 1) + scaledImage.At(2 * r + 1, 2 * lc - 1);
scaledImage.At(2 * r + 1, 2 * lc, 0.25 * sum);
sum = baseImage.At(r, lc) + baseImage.At(r, lc - 1) +
scaledImage.At(2 * r + 1, 2 * lc + 1) + scaledImage.At(2 * r - 1, 2 * lc + 1);
scaledImage.At(2 * r, 2 * lc + 1, 0.25 * sum);
}
sum = baseImage.At(lr, lc) + baseImage.At(lr - 1, lc) +
scaledImage.At(2 * lr + 1, 2 * lc + 1) + scaledImage.At(2 * lr + 1, 2 * lc - 1);
scaledImage.At(2 * lr + 1, 2 * lc, 0.25 * sum);
sum = baseImage.At(lr, lc) + baseImage.At(lr, lc - 1) +
scaledImage.At(2 * lr + 1, 2 * lc + 1) + scaledImage.At(2 * lr - 1, 2 * lc + 1);
scaledImage.At(2 * lr, 2 * lc + 1, 0.25 * sum);
return scaledImage;
}
public static Matrix Matrix_Transpose(Matrix A)
{
Matrix C = new DenseMatrix(A.ColumnCount, A.RowCount);
int i, j;
for (i = 0; i < A.RowCount; i++)
{
for (j = 0; j < A.ColumnCount; j++)
{
C.At(j, i, A.At(i, j));
}
}
return C;
}
public override ulong Create(BitmapData image)
{
var data = new DenseMatrix(32, 32);
using (var unmanaged = new UnmanagedImage(image))
{
using (var filtered = ExtractChannel.Apply(unmanaged))
{
Filter.ApplyInPlace(filtered);
using (var imgdata = Resize.Apply(filtered))
{
unsafe
{
var src = (byte*)imgdata.ImageData.ToPointer();
var offset = imgdata.Stride - imgdata.Width;
var width = imgdata.Width;
var height = imgdata.Height;
for (var y = 0; y < height; y++)
{
for (var x = 0; x < width; x++, src++)
{
data.At(y, x, (float)*src / 255);
}
src += offset;
}
}
}
}
}
var dct = DctMatrix.FastDCT(data);
var vals = new double[dctsize * dctsize];
var valscount = 0;
for (var r = 1; r <= dctsize; r++)
{
for (var c = 1; c <= dctsize; c++)
{
vals[valscount] = dct.At(r, c);
++valscount;
}
}
var sorted = new double[dctsize * dctsize];
Array.Copy(vals, sorted, vals.Length);
Array.Sort(sorted);
var mid = dctsize * dctsize / 2;
var median = (sorted[mid - 1] + sorted[mid]) / 2d;
ulong index = 1;
ulong result = 0;
for (var i = 0; i < dctsize * dctsize; i++)
{
if (vals[i] > median)
{
result |= index;
}
index = index << 1;
}
return result;
}
public static Matrix quat_multi(Matrix A, Matrix B)
{
Matrix C = new DenseMatrix(A.RowCount, A.ColumnCount, 0);
if (A.RowCount != 1 | A.ColumnCount != 4) return C;
if (B.RowCount != 1 | B.ColumnCount != 4) return C;
C.At(0, 0, A.At(0, 0) * B.At(0, 0) - B.At(0, 1) * A.At(0, 1) - B.At(0, 2) * A.At(0, 2) - B.At(0, 3) * A.At(0, 3));
C.At(0, 1, A.At(0, 0) * B.At(0, 1) + B.At(0, 0) * A.At(0, 1) + B.At(0, 3) * A.At(0, 2) - B.At(0, 2) * A.At(0, 3));
C.At(0, 2, A.At(0, 0) * B.At(0, 2) + B.At(0, 0) * A.At(0, 2) - B.At(0, 3) * A.At(0, 1) + B.At(0, 1) * A.At(0, 3));
C.At(0, 3, A.At(0, 0) * B.At(0, 3) + B.At(0, 2) * A.At(0, 1) - B.At(0, 1) * A.At(0, 2) + B.At(0, 0) * A.At(0, 3));
return C;
}
private double[] getCoeffs(bool includeLinear)
{
double[] coeffs = null;
try
{
DenseMatrix X = new DenseMatrix(_calibrationData.Count, 3);
DenseVector Y = new DenseVector(_calibrationData.Count);
int i = 0;
foreach (Spot _spot in _calibrationData)
{
X.At(i, 0, includeLinear ? _spot.s : 0);
X.At(i, 1, _spot.s * _spot.s);
X.At(i, 2, _spot.s * _spot.s * _spot.s);
Y.At(i, _spot.p);
i++;
}
var XT = X.Transpose();
var A = (XT * X).Inverse() * XT * Y;
coeffs = A.ToArray();
}
catch (Exception ex)
{
coeffs = null;
}
return coeffs;
}
private Matrix<double> CameraMatrixFromParameterization(int n0)
{
// Parameters:
// Full: [fx, fy, s, px, py, eaX, eaY, eaZ, Cx, Cy, Cz]
// Center fixed: [fx, fy, s, eaX, eaY, eaZ, Cx, Cy, Cz]
// Aspect fixed: [f, eaX, eaY, eaZ, Cx, Cy, Cz]
//
// Camera : M = KR[I|-C]
double fx = ParametersVector.At(n0 + _fxIdx);
double fy = ParametersVector.At(n0 + _fyIdx);
double sk = ParametersVector.At(n0 + _skIdx);
double px = ParametersVector.At(n0 + _pxIdx);
double py = ParametersVector.At(n0 + _pyIdx);
double eX = ParametersVector.At(n0 + _euXIdx);
double eY = ParametersVector.At(n0 + _euYIdx);
double eZ = ParametersVector.At(n0 + _euZIdx);
Vector<double> euler = new DenseVector(new double[3] { eX, eY, eZ });
double cX = ParametersVector.At(n0 + _cXIdx);
double cY = ParametersVector.At(n0 + _cYIdx);
double cZ = ParametersVector.At(n0 + _cZIdx);
Vector<double> center = new DenseVector(new double[3] { -cX, -cY, -cZ });
Matrix<double> intMat = new DenseMatrix(3, 3);
intMat.At(0, 0, fx);
intMat.At(0, 1, sk);
intMat.At(0, 2, px);
intMat.At(1, 1, fy);
intMat.At(1, 2, py);
intMat.At(2, 2, 1.0);
Matrix<double> rotMat = new DenseMatrix(3, 3);
RotationConverter.EulerToMatrix(euler, rotMat);
Matrix<double> extMat = new DenseMatrix(3, 4);
extMat.SetSubMatrix(0, 0, rotMat);
extMat.SetColumn(3, rotMat * center);
return intMat * extMat;
}
/// <summary>
/// Samples the distribution.
/// </summary>
/// <param name="rnd">The random number generator to use.</param>
/// <param name="degreesOfFreedom">The degrees of freedom (n) for the Wishart distribution.</param>
/// <param name="scale">The scale matrix (V) for the Wishart distribution.</param>
/// <param name="chol">The cholesky decomposition to use.</param>
/// <returns>a random number from the distribution.</returns>
static Matrix<double> DoSample(System.Random rnd, double degreesOfFreedom, Matrix<double> scale, Cholesky<double> chol)
{
var count = scale.RowCount;
// First generate a lower triangular matrix with Sqrt(Chi-Squares) on the diagonal
// and normal distributed variables in the lower triangle.
var a = new DenseMatrix(count, count);
for (var d = 0; d < count; d++)
{
a.At(d, d, Math.Sqrt(Gamma.Sample(rnd, (degreesOfFreedom - d)/2.0, 0.5)));
}
for (var i = 1; i < count; i++)
{
for (var j = 0; j < i; j++)
{
a.At(i, j, Normal.Sample(rnd, 0.0, 1.0));
}
}
var factor = chol.Factor;
return factor*a*a.Transpose()*factor.Transpose();
}
/// <summary>
/// Create a matrix based on this vector in column form (one single column).
/// </summary>
/// <returns>This vector as a column matrix.</returns>
public override Matrix<double> ToColumnMatrix()
{
var matrix = new DenseMatrix(_length, 1);
for (var i = 0; i < _values.Length; i++)
{
matrix.At(i, 0, _values[i]);
}
return matrix;
}
public static Tuple<Vector, Sensors, State> AHRS_LKF_EULER(Sensors Sense, State State, Parameters Param)
{
Vector Attitude = new DenseVector(6, 0);
bool restart = false;
// get sensor data
Matrix m = Sense.m;
Matrix a = Sense.a;
Matrix w = Sense.w;
// Correct magntometers using callibration coefficients
Matrix B = new DenseMatrix(3,3);
B.At(0, 0, Param.magn_coefs.At(0));
B.At(0, 1, Param.magn_coefs.At(3));
B.At(0, 2, Param.magn_coefs.At(4));
B.At(1, 0, Param.magn_coefs.At(5));
B.At(1, 1, Param.magn_coefs.At(1));
B.At(1, 2, Param.magn_coefs.At(6));
B.At(2, 0, Param.magn_coefs.At(7));
B.At(2, 1, Param.magn_coefs.At(8));
B.At(2, 2, Param.magn_coefs.At(2));
Matrix B0 = new DenseMatrix(3, 1);
B0.At(0, 0, Param.magn_coefs.At(9));
B0.At(1, 0, Param.magn_coefs.At(10));
B0.At(2, 0, Param.magn_coefs.At(11));
m = Matrix_Transpose(Matrix_Mult(Matrix_Minus(new DiagonalMatrix(3,3,1),B),Matrix_Minus(Matrix_Transpose(m),B0)));
// Correct accelerometers using callibration coefficients
B.At(0, 0, Param.accl_coefs.At(0));
B.At(0, 1, Param.accl_coefs.At(3));
B.At(0, 2, Param.accl_coefs.At(4));
B.At(1, 0, Param.accl_coefs.At(5));
B.At(1, 1, Param.accl_coefs.At(1));
B.At(1, 2, Param.accl_coefs.At(6));
B.At(2, 0, Param.accl_coefs.At(7));
B.At(2, 1, Param.accl_coefs.At(8));
B.At(2, 2, Param.accl_coefs.At(2));
B0.At(0, 0, Param.accl_coefs.At(9));
B0.At(1, 0, Param.accl_coefs.At(10));
B0.At(2, 0, Param.accl_coefs.At(11));
a = Matrix_Transpose(Matrix_Mult(Matrix_Minus(new DiagonalMatrix(3, 3, 1), B), Matrix_Minus(Matrix_Transpose(a), B0)));
// Correct gyroscopes using callibration coefficients
B.At(0, 0, Param.gyro_coefs.At(0));
B.At(0, 1, Param.gyro_coefs.At(3));
B.At(0, 2, Param.gyro_coefs.At(4));
B.At(1, 0, Param.gyro_coefs.At(5));
B.At(1, 1, Param.gyro_coefs.At(1));
B.At(1, 2, Param.gyro_coefs.At(6));
B.At(2, 0, Param.gyro_coefs.At(7));
B.At(2, 1, Param.gyro_coefs.At(8));
B.At(2, 2, Param.gyro_coefs.At(2));
B0.At(0, 0, Param.gyro_coefs.At(9));
B0.At(1, 0, Param.gyro_coefs.At(10));
B0.At(2, 0, Param.gyro_coefs.At(11));
w = Matrix_Transpose(Matrix_Mult(Matrix_Minus(new DiagonalMatrix(3, 3, 1), B), Matrix_Minus(Matrix_Transpose(w), B0)));
// Get State
Matrix q = State.q;
Matrix dB = State.dB;
Matrix dG = State.dG;
Matrix dw = State.dw;
Matrix P = State.P;
Matrix Wb = Matrix_Transpose(w);
Matrix Ab = Matrix_Transpose(a);
Matrix Mb = Matrix_Transpose(m);
double dT = Param.dT;
//Correct Gyroscopes for estimate biases and scale factor
B.At(0, 0, dB.At(0, 0));
B.At(0, 1, dG.At(0, 0));
B.At(0, 2, dG.At(1, 0));
B.At(1, 0, dG.At(2, 0));
B.At(1, 1, dB.At(1, 0));
B.At(1, 2, dG.At(3, 0));
B.At(2, 0, dG.At(4, 0));
B.At(2, 1, dG.At(5, 0));
B.At(2, 2, dB.At(2, 0));
Matrix Omegab_ib;
Omegab_ib = Matrix_Minus(Matrix_Mult(Matrix_Minus(new DiagonalMatrix(3, 3, 1), B), Wb),dw);
if (q.At(0, 0).ToString() == "NaN")
{
restart = true;
}
//Quternion calculation
q = mrotate(q, Omegab_ib, dT);
if (q.At(0,0).ToString() == "NaN")
{
restart = true;
}
//DCM calculation
Matrix Cbn = quat_to_DCM(q);
//Gyro Angles
Matrix angles = dcm2angle(Cbn);
double Psi = (double) angles.At(0, 0);
double Theta = (double) angles.At(0, 1);
double Gamma = (double) angles.At(0, 2);
//Acceleration Angles
double ThetaAcc = (double)Math.Atan2(Ab.At(0, 0), Math.Sqrt(Ab.At(1, 0) * Ab.At(1, 0) + Ab.At(2, 0) * Ab.At(2, 0)));
double GammaAcc = (double)-Math.Atan2(Ab.At(1, 0), Math.Sqrt(Ab.At(0, 0) * Ab.At(0, 0) + Ab.At(2, 0) * Ab.At(2, 0)));
//Horizontal projection of magnetic field
angles.At(0, 0, 0);
angles.At(0, 1, ThetaAcc);
angles.At(0, 2, GammaAcc);
Matrix Cbh = angle2dcm(angles);
Matrix Mh = Matrix_Mult(Matrix_Transpose(Cbh), Mb);
//Magnetic Heading
double PsiMgn = (double)(-Math.Atan2(Mh.At(1,0),Mh.At(0,0)) + Param.declination);
//System matrix
Matrix A = new DenseMatrix (15, 15, 0);
Matrix I = new DiagonalMatrix (15, 15, 1);
A.At(0, 3, 1);
A.At(1, 4, 1);
A.At(2, 5, 1);
A.At(0, 6, 1);
A.At(1, 7, 1);
A.At(2, 8, 1);
A.At(0, 9, Omegab_ib.At(1, 0));
A.At(0, 10, Omegab_ib.At(2, 0));
A.At(1, 11, Omegab_ib.At(0, 0));
A.At(1, 12, Omegab_ib.At(2, 0));
A.At(2, 13, Omegab_ib.At(0, 0));
A.At(2, 14, Omegab_ib.At(1, 0));
Matrix F = Matrix_Plus(I, Matrix_Const_Mult(A,dT));
//Measurment Matrix
double dPsi = Psi - PsiMgn;
if (dPsi > Math.PI) dPsi = (double)(dPsi - 2 * Math.PI);
if (dPsi < -Math.PI) dPsi = (double)(dPsi + 2 * Math.PI);
double dTheta = Theta - ThetaAcc;
if (dTheta > Math.PI) dTheta = (double)(dTheta - 2 * Math.PI);
if (dTheta < -Math.PI) dTheta = (double)(dTheta + 2 * Math.PI);
double dGamma = Gamma - GammaAcc;
if (dGamma > Math.PI) dGamma = (double)(dGamma - 2 * Math.PI);
if (dGamma < -Math.PI) dGamma = (double)(dGamma + 2 * Math.PI);
Matrix z = new DenseMatrix(3, 1, 1);
z.At(0, 0, dGamma);
z.At(1, 0, dTheta);
z.At(2, 0, dPsi);
Matrix H = new DenseMatrix(3, 15, 0);
H.At(0, 0, 1);
H.At(1, 1, 1);
H.At(2, 2, 1);
if (Math.Abs(Math.Sqrt(Ab.At(0, 0) + Ab.At(1, 0) + Ab.At(2, 0))) > Param.accl_threshold)
{
H.At(0, 0, 0);
H.At(1, 1, 0);
}
//Kalman Filter
Matrix Q = State.Q;
Matrix R = State.R;
P = Matrix_Plus(Matrix_Mult(Matrix_Mult(F, P), Matrix_Transpose(F)),Q);
if (P.At(0, 0).ToString() == "NaN")
{
P = new DiagonalMatrix(15, 15, (double)Math.Pow(10, -8));
P.At(0, 0, (double)Math.Pow(10, -1));
P.At(1, 1, (double)Math.Pow(10, -1));
P.At(2, 2, (double)Math.Pow(10, -1));
P.At(3, 3, (double)Math.Pow(10, -3));
P.At(4, 4, (double)Math.Pow(10, -3));
P.At(5, 5, (double)Math.Pow(10, -3));
restart = true;
}
Tuple<Matrix, Matrix> KF_result;
KF_result = KF_Cholesky_update(P,z,R,H);
Matrix xf = KF_result.Item1;
P = KF_result.Item2;
Matrix df_hat = new DenseMatrix(3, 1, 0);
df_hat.At(0, 0, xf.At(0, 0));
df_hat.At(1, 0, xf.At(1, 0));
df_hat.At(2, 0, xf.At(2, 0));
Matrix dw_hat = new DenseMatrix(3, 1, 0);
dw_hat.At(0, 0, xf.At(3, 0));
dw_hat.At(1, 0, xf.At(4, 0));
dw_hat.At(2, 0, xf.At(5, 0));
Matrix dB_hat = new DenseMatrix(3, 1, 0);
dB_hat.At(0, 0, xf.At(6, 0));
dB_hat.At(1, 0, xf.At(7, 0));
dB_hat.At(2, 0, xf.At(8, 0));
Matrix dG_hat = new DenseMatrix(6, 1, 0);
dG_hat.At(0, 0, xf.At(9, 0));
dG_hat.At(1, 0, xf.At(10, 0));
dG_hat.At(2, 0, xf.At(11, 0));
dG_hat.At(3, 0, xf.At(12, 0));
dG_hat.At(4, 0, xf.At(13, 0));
dG_hat.At(5, 0, xf.At(14, 0));
dw = Matrix_Plus(dw, dw_hat);
dB = Matrix_Plus(dB, dB_hat);
dG = Matrix_Plus(dG, dG_hat);
Matrix dCbn = new DenseMatrix(3, 3, 0);
dCbn.At(0, 1, -df_hat.At(2, 0));
dCbn.At(0, 2, df_hat.At(1, 0));
dCbn.At(1, 0, df_hat.At(2, 0));
dCbn.At(1, 2, -df_hat.At(0, 0));
dCbn.At(2, 0, -df_hat.At(1, 0));
dCbn.At(2, 1, df_hat.At(0, 0));
Cbn = Matrix_Mult(Matrix_Plus(new DiagonalMatrix(3, 3, 1), dCbn), Cbn);
if (Cbn.At(0, 0).ToString() == "NaN")
{
restart = true;
}
if (dcm2quat(Cbn).At(0, 0).ToString() == "NaN")
{
restart = true;
}
q = dcm2quat(Cbn);
if (q.At(0, 0).ToString() == "NaN")
{
restart = true;
}
q = quat_norm(q);
if (q.At(0, 0).ToString() == "NaN")
{
restart = true;
}
Attitude.At(0, Psi);
Attitude.At(1, Theta);
Attitude.At(2, Gamma);
Attitude.At(3, PsiMgn);
Attitude.At(4, ThetaAcc);
Attitude.At(5, GammaAcc);
State.q = q;
State.dG = dG;
State.dB = dB;
State.dw = dw;
State.P = P;
Sense.w = Matrix_Transpose(Omegab_ib);
Sense.a = a;
Sense.m = m;
if (restart)
{
Matrix Initia_quat = new DenseMatrix(1, 4, 0);
Initia_quat.At(0, 0, 1);
State = new Kalman_class.State(ACCLERATION_NOISE, MAGNETIC_FIELD_NOISE, ANGULAR_VELOCITY_NOISE,
Math.Pow(10, -6), Math.Pow(10, -15), Math.Pow(10, -15), Initia_quat);
}
return new Tuple <Vector, Sensors, State>(Attitude, Sense, State);
}
/// <summary>
/// Outer product of two vectors
/// </summary>
/// <param name="u">First vector</param>
/// <param name="v">Second vector</param>
/// <returns>Matrix M[i,j] = u[i]*v[j] </returns>
/// <exception cref="ArgumentNullException">If the u vector is <see langword="null" />.</exception>
/// <exception cref="ArgumentNullException">If the v vector is <see langword="null" />.</exception>
public static DenseMatrix OuterProduct(DenseVector u, DenseVector v)
{
if (u == null)
{
throw new ArgumentNullException("u");
}
if (v == null)
{
throw new ArgumentNullException("v");
}
var matrix = new DenseMatrix(u.Count, v.Count);
CommonParallel.For(0, u.Count, (a, b) =>
{
for (int i = a; i < b; i++)
{
for (var j = 0; j < v.Count; j++)
{
matrix.At(i, j, u._values[i]*v._values[j]);
}
}
});
return matrix;
}
public static Matrix angle2dcm(Matrix angles)
{
Matrix DCM = new DenseMatrix(3, 3, 0);
if (angles.RowCount != 1 | angles.ColumnCount != 3) return DCM;
double Cos1 = (double)Math.Cos(angles.At(0, 0));
double Cos2 = (double)Math.Cos(angles.At(0, 1));
double Cos3 = (double)Math.Cos(angles.At(0, 2));
double Sin1 = (double)Math.Sin(angles.At(0, 0));
double Sin2 = (double)Math.Sin(angles.At(0, 1));
double Sin3 = (double)Math.Sin(angles.At(0, 2));
DCM.At(0, 0, Cos2 * Cos1);
DCM.At(0, 1, Cos2 * Sin1);
DCM.At(0, 2, -Sin2);
DCM.At(1, 0, Sin3 * Sin2 * Cos1 - Cos3 * Sin1);
DCM.At(1, 1, Sin3 * Sin2 * Sin1 + Cos3 * Cos1);
DCM.At(1, 2, Sin3 * Cos2);
DCM.At(2, 0, Cos3 * Sin2 * Cos1 + Sin3 * Sin1);
DCM.At(2, 1, Cos3 * Sin2 * Sin1 - Sin3 * Cos1);
DCM.At(2, 2, Cos3 * Cos2);
return DCM;
}
/// <summary>
/// Helper method to calculate an approximation of the Jacobian.
/// </summary>
/// <param name="f">The function.</param>
/// <param name="x0">The argument (initial guess).</param>
/// <param name="y0">The result (of initial guess).</param>
static Matrix<double> CalculateApproximateJacobian(Func<double[], double[]> f, double[] x0, double[] y0)
{
int dim = x0.Length;
var B = new DenseMatrix(dim);
var x = new double[dim];
Array.Copy(x0, 0, x, 0, dim);
for (int j = 0; j < dim; j++)
{
double h = Math.Abs(x0[j])*1.0e-4;
if (h == 0.0)
{
h = 1.0e-4;
}
var xj = x[j];
x[j] = xj + h;
double[] y = f(x);
x[j] = xj;
for (int i = 0; i < dim; i++)
{
B.At(i, j, (y[i] - y0[i])/h);
}
}
return B;
}
public static Matrix dcm2angle(Matrix DCM)
{
Matrix angles = new DenseMatrix(1,3,0);
if (DCM.RowCount != 3 | DCM.ColumnCount != 3) return angles;
angles.At(0, 0, (double)(Math.Atan2(DCM.At(0, 1), DCM.At(0, 0))));
angles.At(0, 1, (double)-(Math.Atan2(DCM.At(0, 2), Math.Sqrt(1 - DCM.At(0, 2) * DCM.At(0, 2)))));
angles.At(0, 2, (double)(Math.Atan2(DCM.At(1, 2), DCM.At(2, 2))));
return angles;
}
