空间后方交会程序

空间后方交会的误差方程式：

可以简写为：
写成矩阵形式：

根据误差方程式列出法方程式：

整理可得：

其中：

窗口展示：

 

以下是代码实现：

Form1:

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Drawing.Drawing2D;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;

namespace WindowsFormsApp1
{
    public partial class Form1 : Form
    {
        public Form1()
        {
            InitializeComponent();
        }

        private void Form1_Load(object sender, EventArgs e)
        {

        }

        private void button1_Click(object sender, EventArgs e)
        {
            //类型转化
            double[] x = { Convert.ToDouble(t1.Text), Convert.ToDouble(t6.Text), Convert.ToDouble(t11.Text), Convert.ToDouble(t16.Text) };

            double[] y = { Convert.ToDouble(t2.Text), Convert.ToDouble(t7.Text), Convert.ToDouble(t12.Text), Convert.ToDouble(t17.Text) };

            double[] X = { Convert.ToDouble(t3.Text), Convert.ToDouble(t8.Text), Convert.ToDouble(t13.Text), Convert.ToDouble(t18.Text) };

            double[] Y = { Convert.ToDouble(t4.Text), Convert.ToDouble(t9.Text), Convert.ToDouble(t14.Text), Convert.ToDouble(t19.Text) };

            double[] Z = { Convert.ToDouble(t5.Text), Convert.ToDouble(t10.Text), Convert.ToDouble(t15.Text), Convert.ToDouble(t20.Text) };

            double f = 0.15324, _m = 40000.0;

            for (int i = 0; i < 4; i++)    //单位换算

            {

                x[i] = x[i] / 1000;

                y[i] = y[i] / 1000;

            }

            // 定义外方位元素，并附初值

            double Xs, Ys, Zs, phi = 0, omiga = 0, kappa = 0;

            Xs = (X[0] + X[1] + X[2] + X[3]) / 4.0;

            Ys = (Y[0] + Y[1] + Y[2] + Y[3]) / 4.0;

            Zs = _m * f;

            // 定义x,y近似值,即计算值

            double[] _x = new double[4];

            double[] _y = new double[4];

            // 定义共线方程中的分子分母项，便于计算

            double[] _X = new double[4];

            double[] _Y = new double[4];

            double[] _Z = new double[4];

            // 定义旋转矩阵R

            double[,] R = new double[3, 3];

            double[,] L = new double[8, 1];//误差方程中的常数项

            double[,] XX = new double[6, 1];//X向量