本文整理汇总了C#中MathNet.Numerics.LinearAlgebra.Single.DenseVector.At方法的典型用法代码示例。如果您正苦于以下问题:C# DenseVector.At方法的具体用法?C# DenseVector.At怎么用?C# DenseVector.At使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类MathNet.Numerics.LinearAlgebra.Single.DenseVector
的用法示例。
在下文中一共展示了DenseVector.At方法的1个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C#代码示例。
示例1: AHRS_LKF_EULER
public static Tuple<Vector, Sensors, State> AHRS_LKF_EULER(Sensors Sense, State State, Parameters Param)
{
Vector Attitude = new DenseVector(6, 0);
// 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);
float 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);
//Quternion calculation
q = mrotate(q, Omegab_ib, dT);
//DCM calculation
Matrix Cbn = quat_to_DCM(q);
//Gyro Angles
Matrix angles = dcm2angle(Cbn);
float Psi = (float) angles.At(0, 0);
float Theta = (float) angles.At(0, 1);
float Gamma = (float) angles.At(0, 2);
//Acceleration Angles
//.........这里部分代码省略.........