/// <summary>
/// 功能：将外部程序仿真的姿态转换为陀螺角速度
/// 日期：2017.09.06
/// 作者：GZC
/// </summary>
private void button11_Click(object sender, EventArgs e)
{
//星敏参数
mAtt.sig_ST = double.Parse(textBox4.Text);//星敏误差(单位：角秒)
//陀螺漂移
string[] strW = textBox5.Text.Split(',');
double[] wBias = new double[3];
for (int i = 0; i < 3; i++)
{
wBias[i] = double.Parse(strW[i]);
}
mAtt.wBiasA = wBias;
//漂移噪声
mAtt.sigu = double.Parse(textBox6.Text) * 1e-9;
//陀螺噪声
mAtt.sigv = double.Parse(textBox7.Text) * 1e-5;
if (!File.Exists(path + "\\ATT.txt"))
{
ShowInfo("没有ATT.txt文件");
MessageBox.Show("请设置真实数据路径（包含ATT.txt文件）", "警告", MessageBoxButtons.OK);
return;
}
DLLImport.ExternalData(path, mAtt);
}
/// <summary>
/// 珞珈一号求A星敏相对B星敏安装
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void button21_Click(object sender, EventArgs e)
{
//姿态文件路径
mAtt.sSimAtt = textBox29.Text;
progressBar1.Minimum = 0;
progressBar1.Maximum = 100;
progressBar1.Value = 40;
if (!File.Exists(path + "\\qa.txt") || !File.Exists(path + "\\qb.txt") || !File.Exists(path + "\\gyro.txt"))
{
ShowInfo("没有qa,qb,gyro文件");
MessageBox.Show("请设置真实数据路径（包含qa,qb,gyro的txt文件）", "警告", MessageBoxButtons.OK);
return;
}
ShowInfo("开始求取相对安装...");
DLLImport.luojia1StarAlin(path);
ShowInfo("相对安装求取完毕");
progressBar1.Value = 100;
}
/// <summary>
/// 珞珈一号滤波
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void button20_Click(object sender, EventArgs e)
{
//星敏参数
mAtt.sig_ST = double.Parse(textBox14.Text);//星敏误差(单位：角秒)
//陀螺噪声
mAtt.sigv = double.Parse(textBox28.Text) * 1e-4;
//漂移噪声
mAtt.sigu = double.Parse(textBox26.Text) * 1e-5;
//星敏安装矩阵
mAtt.install = textBox13.Text;
//姿态文件路径
mAtt.sSimAtt = textBox29.Text;
progressBar1.Minimum = 0;
progressBar1.Maximum = 100;
progressBar1.Value = 40;
if (!File.Exists(path + "\\qa.txt") || !File.Exists(path + "\\qb.txt") || !File.Exists(path + "\\gyro.txt"))
{
ShowInfo("没有qa,qb,gyro文件");
MessageBox.Show("请设置真实数据路径（包含qa,qb,gyro的txt文件）", "警告", MessageBoxButtons.OK);
return;
}
ShowInfo("开始姿态确定...");
if (radioButton5.Checked == true)
{
DLLImport.luojia1AttitudeDeter(path, mAtt, false);
}
else if (radioButton6.Checked == true)
{
DLLImport.luojia1AttitudeDeter(path, mAtt, true);
}
ShowInfo("姿态确定完毕");
progressBar1.Value = 100;
}
/// <summary>
/// 真实数据开始滤波
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void button15_Click(object sender, EventArgs e)
{
//首先判断选中了哪些星敏和陀螺
if (checkBox1.Checked)
{
starGyro.isA = true;
}
else
{
starGyro.isA = false;
}
if (checkBox2.Checked)
{
starGyro.isB = true;
}
else
{
starGyro.isB = false;
}
if (checkBox3.Checked)
{
starGyro.isC = true;
}
else
{
starGyro.isC = false;
}
if (checkBox4.Checked)
{
starGyro.isG11 = true;
}
else
{
starGyro.isG11 = false;
}
if (checkBox5.Checked)
{
starGyro.isG12 = true;
}
else
{
starGyro.isG12 = false;
}
if (checkBox6.Checked)
{
starGyro.isG13 = true;
}
else
{
starGyro.isG13 = false;
}
if (checkBox7.Checked)
{
starGyro.isG21 = true;
}
else
{
starGyro.isG21 = false;
}
if (checkBox8.Checked)
{
starGyro.isG22 = true;
}
else
{
starGyro.isG22 = false;
}
if (checkBox9.Checked)
{
starGyro.isG23 = true;
}
else
{
starGyro.isG23 = false;
}
if (checkBox10.Checked)
{
starGyro.isG31 = true;
}
else
{
starGyro.isG31 = false;
}
if (checkBox11.Checked)
{
starGyro.isG32 = true;
}
else
{
starGyro.isG32 = false;
}
if (checkBox12.Checked)
{
starGyro.isG33 = true;
}
else
{
starGyro.isG33 = false;
}
if (checkBox13.Checked)
{
starGyro.isJitter = true;
}
else
{
starGyro.isJitter = false;
}
//获取星敏陀螺频率和总时长
//mAtt.totalT = int.Parse(textBox2.Text); //string转数值的第2种转换方式
mAtt.freqQ = Convert.ToInt32(textBox15.Text);
mAtt.freqG = Convert.ToInt32(textBox17.Text); //string转数值的第1种转换方式
//星敏参数
mAtt.sig_ST = double.Parse(textBox16.Text); //星敏误差(单位：角秒)
//陀螺噪声
mAtt.sigv = double.Parse(textBox19.Text) * 1e-4;
//陀螺漂移
double[] wBias = new double[3];
for (int i = 0; i < 3; i++)
{
wBias[i] = double.Parse(textBox20.Text);
}
mAtt.wBiasA = wBias;
//漂移噪声
mAtt.sigu = double.Parse(textBox21.Text) * 1e-5;
//高频角位移测量频率
mAtt.ADSfreq = int.Parse(textBox25.Text);
//星敏安装矩阵
mAtt.install = textBox23.Text;
progressBar1.Minimum = 0;
progressBar1.Maximum = 100;
progressBar1.Value = 40;
if (!File.Exists(path + "\\ManeuverData_All.txt"))
{
ShowInfo("没有ManeuverData_All.txt文件");
MessageBox.Show("请设置真实数据路径（包含ManeuverData_All.txt文件）", "警告", MessageBoxButtons.OK);
return;
}
ShowInfo("开始姿态确定...");
DLLImport.ExternalFileAttitudeDeter(path, mAtt, starGyro);
ShowInfo("姿态仿真完毕");
progressBar1.Value = 100;
}
/// <summary>
/// 功能：主动推扫模式仿真
/// 日期：2017.10.18
/// 作者：GZC
/// </summary>
private void button8_Click(object sender, EventArgs e)
{
if (path == null)
{
ShowInfo("失败未设置文件保存路径");
MessageBox.Show("请设置仿真文件保存路径", "警告", MessageBoxButtons.OK);
return;
}
if (nSim2 == 0)
{
button8.Text = "主动重新仿真";
nSim2++;
}
progressBar1.Minimum = 0;
progressBar1.Maximum = 100;
progressBar1.Value = 10;
ShowInfo("根据不同频率星敏陀螺测量数据仿真");
//获取星敏陀螺频率和总时长
mAtt.totalT = int.Parse(textBox2.Text); //string转数值的第2种转换方式
mAtt.freqQ = Convert.ToInt32(textBox8.Text);
mAtt.freqG = Convert.ToInt32(textBox9.Text); //string转数值的第1种转换方式
mAtt.nQuat = mAtt.freqQ * mAtt.totalT; //四元数个数
mAtt.nGyro = mAtt.freqG * mAtt.totalT; //陀螺个数，一般较四元数多
//姿态稳定度
string[] strStab = textBox1.Text.Split(',');
double[] stabW = new double[3];
for (int i = 0; i < 3; i++)
{
stabW[i] = double.Parse(strStab[i]) * 1e-4;
}
mAtt.stabW = stabW;
if (Math.Abs(stabW[0]) > 0.01 || Math.Abs(stabW[1]) > 0.01 || Math.Abs(stabW[1]) > 0.01)
{
ShowInfo("重新输入姿态稳定度");
MessageBox.Show("请将姿态稳定度范围限制在0.01°/s范围内", "警告", MessageBoxButtons.OK);
return;
}
//得到初始四元数
string[] strQ = textBox3.Text.Split(',');
double[] qInitial = new double[4];
for (int i = 0; i < 4; i++)
{
qInitial[i] = double.Parse(strQ[i]);
}
mAtt.qInitial = qInitial;
double qSquare = Math.Sqrt(Math.Pow(qInitial[0], 2) + Math.Pow(qInitial[1], 2) + Math.Pow(qInitial[2], 2) + Math.Pow(qInitial[3], 2));
if (qInitial[0] > 1 || qInitial[1] > 1 || qInitial[2] > 1 || qInitial[3] > 1 || qSquare > 1.01 || qSquare < 0.99)
{
ShowInfo("重新输入姿态四元数");
MessageBox.Show("四元数的和应为1", "警告", MessageBoxButtons.OK);
return;
}
//星敏参数
mAtt.sig_ST = double.Parse(textBox4.Text);//星敏误差(单位：角秒)
//陀螺漂移
string[] strW = textBox5.Text.Split(',');
double[] wBias = new double[3];
for (int i = 0; i < 3; i++)
{
wBias[i] = double.Parse(strW[i]);
}
mAtt.wBiasA = wBias;
//漂移噪声
mAtt.sigu = double.Parse(textBox6.Text) * 1e-9;
//sigu = Math.Sqrt(sigu) * 1e-10;
//陀螺噪声
mAtt.sigv = double.Parse(textBox7.Text) * 1e-5;
//sigv = Math.Sqrt(sigv) * 1e-7;
//陀螺尺度和安装
string[] strSarr = textBox10.Text.Split(',');
double[] sArr = new double[9];
for (int i = 0; i < 9; i++)
{
sArr[i] = double.Parse(strSarr[i]) * 1e-6;
}
mAtt.sArr = sArr;
//主动推扫前后多余时间
double[] BeforeAfterT = new double[2];
string[] strBAT = textBox18.Text.Split(',');
for (int i = 0; i < 2; i++)
{
BeforeAfterT[i] = double.Parse(strBAT[i]);
}
//注意时间维度
double[] qTrueC = new double[5 * mAtt.nQuat]; double[] qMeasC = new double[5 * mAtt.nQuat];
double[] wTrueC = new double[4 * mAtt.nGyro]; double[] wMeasC = new double[4 * mAtt.nGyro];
double[] qNoise = new double[3 * mAtt.nQuat];
progressBar1.Value = 20;
ShowInfo("开始单向卡尔曼滤波...");
DLLImport.attitudeSimulationStruct(mAtt, path, qTrueC, qMeasC, wTrueC, wMeasC, qNoise);
progressBar1.Value = 40;
double[] dqOut = new double[3 * mAtt.nQuat];
double[] xest_store = new double[15 * mAtt.nGyro];
DLLImport.attitudeDeterActivePushbroomStruct(mAtt, BeforeAfterT, path,
qTrueC, qMeasC, 0, wTrueC, wMeasC, dqOut, xest_store);
dq = dqOut; qNs = qNoise; xestAll = xest_store; qMeas = qMeasC;
dqOut = new double[3 * mAtt.nQuat];
xest_store = new double[15 * mAtt.nGyro];
progressBar1.Value = 70;
ShowInfo("开始双向卡尔曼滤波...");
DLLImport.attitudeDeterActivePushbroomStruct(mAtt, BeforeAfterT, path,
qTrueC, qMeasC, 1, wTrueC, wMeasC, dqOut, xest_store);
dq2 = dqOut; xestAll2 = xest_store;
dqOut = null; qNoise = null; xest_store = null;
progressBar1.Value = 100;
ShowInfo("仿真完成！");
button3.Enabled = true;
button4.Enabled = true;
button5.Enabled = true;
button6.Enabled = true;
button7.Enabled = true;
button9.Enabled = true;
button10.Enabled = true;
}
