Esempio n. 1
0
        // Функция, по моментам измерения и указанным ориентирам (которые
        // возвращает предыдущая функция) и заданным начальным условиям
        // вычисляющая результаты измерений -- углы ФИ в заданный момент
        // времени по отношению к заданному ориентиру. Иначе говоря,
        // функция возвращает математичесскую модель вектора измерений z.
        //
        // Вычисление результатов измерений отделено от предыдущей функции
        // потому, что предыдущая функция используется только при вычислении
        // вектора измерений R, так как определяет, в какие моменты времени
        // и по каким ориентирам измерения производятся. Эта информация
        // остается известной в процессе оценки вектора оцениваемых
        // параметров (тета), содержащего начальные координаты и скорости КА
        // в момент времени t = 0, и вычисляется только один раз, до начала
        // работы метода максимального правдоподобия.
        //
        // Данная же функция используется на каждой итерации метода
        // максимального правдоподобия (ММП) при вычислении z от тета, а тета
        // изменяется на каждой итерации.
        //
        // В процессе работы ММП, вычисляются координаты КА в моменты измерений
        // при других начальных условиях, при этом новые значения координат
        // КА в моменты измерений будут отличаться от предыщих, и, следовательно,
        // получим другие значения углов ФИ, которые являются математической
        // моделью вектора измерений z.
        //
        // Эта функция также использует интегрирование системы дифференциальных
        // уравнений, описывающих движение и изменение скорости КА, но в ней
        // уже не ищутся моменты допустимых измерений, а, наоборот, ищутся
        // координаты КА в уже заданные моменты времени.
        //
        // Моменты времени в gaugings расположены в возрастающем порядке.
        //
        // Функция возвращает массив результатов измерений -- мат. модель вектора
        // измерений z, зависящую от компонентов вектора тета -- xk, yk, vx, vy.
        //
        // gaugings -- массив, содержащий моменты и ориентиры измерений(gaugings)
        // xk -- абсцисса КА в начальный момент времени t = 0, она же тета1
        // yk -- ордината КА в начальный момент времени t = 0, она же тета2
        // vx -- горизонтальная скорость КА в начальный момент времени t = 0, она же тета3
        // vy -- вертикальная скорость КА в начальный момент времени t = 0, она же тета4
        // x0s, y0s -- координаты ориентиров: x0s -- абсциссы, y0s -- ординаты
        public static double[] getZ(
            Gauging[] gaugings,
            double xk, double yk, double vx, double vy,
            double[] x0s, double[] y0s
            )
        {
            // Результат заносится сюда -- в массив из count измерений
            double[] z = new double[gaugings.Length];

            // Время модели, определяет, для какого момента вычислены
            // текущие координаты/скорости КА -- xk, yk, vx, vy.
            // Также равно времени предыдущего измерения, изначально
            // равно нулю.
            int t = 0;

            // Проходя последовательно по каждому измерению
            for (int i = 0; i < gaugings.Length; i++)
            {
                // Вычислим время, через которое КА проведет измерение i.
                // (duration -- длительность)
                int duration = gaugings[i].t - t;

                // В какие моменты времени вычислять (с каким шагом
                // интегрировать) уравнения движения КА для вычисления
                // его положения во время i-го измерения
                //
                // Содержит целые числа от нуля до разности между
                // временем, для которого заданы текущие знаения
                // координат/скоростей КА до момента i-го
                // измерения. Тем самым мы переходим от значений
                // координат/скоростей КА в момент времени t, к их
                // значениям в момент i-го измерения.
                //
                // Массив имеет тип double[] для совместимости, однако
                // в нем находятся только целые значения.
                double[] xs = new double[duration + 1];

                for (int sec = 0; sec <= duration; sec++)
                {
                    xs[sec] = sec;
                }

                // Находим координаты/скорости КА к моменту времени gaugings[i].t
                double[,] nextPosition =
                    RungeCutta.Integrate(derivatives, new double[] { xk, yk, vx, vy }, xs);

                // Обновляем текущие значения координат/скоростей
                // Значения координат/скоростей на последнем шаге интегрирования,
                // равном duration, соответствуют значениям к моменту i-го
                // измерения.
                xk = nextPosition[duration, 0];
                yk = nextPosition[duration, 1];
                vx = nextPosition[duration, 2];
                vy = nextPosition[duration, 3];


                // Теперь значения координат вычислены на момент времени
                // i-го измерения.
                t = gaugings[i].t;


                // Остается лишь записать в массив результат измерения угла ФИ.
                // Ориентир уже определен.
                z[i] = phi(xk, yk, x0s[gaugings[i].landmark], y0s[gaugings[i].landmark]);
            }

            // Возвращаем вычисленные по модели результаты измерений
            return(z);
        }
Esempio n. 2
0
        // Функция, возвращающая моменты времени, в которые выполняются
        // измерения, и по какому ориентиру измерение каждое измерение
        // выполняется, при заданных условиях, передаваемых через
        // параметры: начальных координатах и скоростях КА, координатах
        // ориентиров, интервале измерений и методе выбора ориентира,
        // по которому нужно делать измерение, если в данный момент
        // можно выполнить измерение по нескольким ориентирам.
        //
        // Также задается count -- планируемое количество измерений. Если
        // за время, в течение которого КА находится над ориентирами, такое
        // количество измерений выполнить невозможно, функция выбрасывает
        // исключение
        //
        // Возвращает массив Gauging[], то есть массив записей вида
        // <время, номер ориентира>. В один момент времени измерение может
        // быть выполнено только по одному ориентиру.
        //
        // xk, yk -- координаты КА в условный момент времени 0
        // vx, xy -- скорости КА в условный момент времени 0
        // x0s, y0s -- координаты ориентиров: x0s -- абсциссы, y0s -- ординаты
        // interval -- интервал времени в секундах, с которым производятся измерения
        // count -- количество измерений, которое планируется выполнить
        // selection -- какой ориентир выбрать для измерения, если КА находится
        // в области, где угол ГАММА не превышает 70 градусов для нескольких
        // ориентиров
        public static Gauging[] getGaugings(
            double xk, double yk, double vx, double vy,
            double[] x0s, double[] y0s,
            int interval, int count,
            LandmarkSelection selection
            )
        {
            // Результат заносится сюда -- в массив из count измерений
            Gauging[] gaugings = new Gauging[count];


            // Сколько измерений уже сделано(done)
            //
            // Когда равно планируемому количеству измерений count,
            // цикл поиска моментов измерений завершается
            //
            // В массиве gaugings показывает индекс, куда записывать
            // момент, ориентир и результат следующего измерения
            //
            // Если измерения производятся поочередно по нескольким
            // ориентирам (selection === ALTERNATE), позволяет
            // поочередно менять ориентиры
            int done = 0;


            // За количество(count) ориентиров(LandMark) примем длину
            // массива абсцисс ориентиров
            int lmCount = x0s.Length;


            // В какие моменты времени вычислять (с каким шагом
            // интегрировать) уравнения движения КА для вычисления
            // его положения во время следующего измерения (через
            // интервал измерений interval)
            //
            // Начинается с нуля, а не с текущего времени t, так
            // как RungeCutta.Integrate берет только разности
            // между моментами времени xs[1] - xs[0], xs[2] - xs[1], ...
            //
            // Таким образом, [0, 1, 2, ..., interval] эквивалентно
            // [t, t + 1, t + 2, ..., t + interval] с точки зрения
            // RungeCutta.Integrate. Если передать в RungeCutta.Integrate
            // [0, 1, 2, ..., interval] и координаты/скорости КА на
            // момент времени t, то фактически будут вычислены
            // координаты/скорости в моменты [t, t + 1, t + 2, ...,
            // t + interval]
            //
            // Массив имеет тип double[] для совместимости, однако
            // в нем находятся только целые значения.
            double[] xs = new double[interval + 1];

            for (int sec = 0; sec <= interval; sec++)
            {
                xs[sec] = sec;
            }


            // Текущий момент времени, показывает время в модели,
            // прошедшее с начального момента, для которого начальные
            // координаты и скорости КА заданы извне (переданы через
            // параметры)
            //
            // Время будем измерять в целых секундах времени
            //
            // В начальный момент времени t = 0
            int t = 0;


            for (; ;)
            {
                // В этот список будут помещаться номера ориентиров, над
                // которыми сейчас (при текущем значении t) пролетает КА
                //
                // По идее эту переменную нужно было назвать "availableLandmarks"
                // (доступные ориентиры), но для краткости она называется просто
                // landmarks
                //
                // КА пролетает над ориентиром -- то есть находится в данный
                // момент в области, для которой угол ГАММА относительно
                // данного ориентира не  превышает 70 градусов
                //
                // (где, согласно заданию, измерения могут быть проведены без
                // дополнительных погрешностей -- из других точек мы просто
                // не будем делать измерения)
                List <int> landmarks = new List <int>();

                for (int lm = 0; lm < lmCount; lm++)
                {
                    if (inBand(xk, yk, x0s[lm], y0s[lm]))
                    {
                        // Внесем lm -- номер (индекс) ориентира в список
                        landmarks.Add(lm);
                    }
                }

                // Если КА сейчас пролетает хотя бы над одним из ориентиров,
                // записать данный момент времени и выбрать ориентир, по
                // которому сейчас должно быть выполнено измерение угла ФИ
                //
                // Угол ФИ -- измеряемая величина
                if (landmarks.Count > 0)
                {
                    // Номер (индекс) выбранного ориентира в списке всех ориентиров
                    //
                    // Самого списка всех ориентиров нет, под ним подразумеваются
                    // массивы x0s/y0s координат ориентиров
                    int selectedLandmark;

                    // При поочередном выборе ориентиров
                    if (selection == LandmarkSelection.ALTERNATE)
                    {
                        // Из доступных ориентиров выберем доступный ориентир с
                        // индексом, равным остатку от деления количества сделанных
                        // измерений done на количество доступных ориентиров
                        // landmarks.Count. Результат деления будет одним из
                        // {0, 1, 2, ..., landMarks.Count - 1}, каждое из этих
                        // чисел является допустимым индексом в списке доступных
                        // ориентиров. Так как done на следующей итерации увеличится
                        // на единицу, то при таком же множестве ориентиров, над
                        // которыми пролетает КА, будет выбран следующий ориентир.
                        // При небольшом интервале измерений множество ориентиров,
                        // над которыми пролетает КА в момент следующего измерения,
                        // с большой вероятностью останется таким же.
                        //
                        // Индекс выбранного ориентира в списке доступных ориентиров
                        // (над которыми сейчас пролетает КА)
                        int selectedLandmarkInAvailableLandmarks = done % landmarks.Count;

                        // По индексу выбранного ориентира в списке доступных
                        // ориентиров найдем индекс этого ориентира в списке всех
                        // ориентиров.
                        selectedLandmark = landmarks[selectedLandmarkInAvailableLandmarks];
                    }
                    // Иначе выбираем ориентир, для которого угол ГАММА в текущий
                    // момент времени наименьший.
                    //
                    // Значение selection может быть только NEAREST, если оно
                    // не равно ALTERNATE, поэтому проверять
                    // if (selection == LandmarkSelection.ALTERNATE) не нужно.
                    else
                    {
                        // Сначала положим выбранный ориентир равным первому из
                        // доступных; хотя бы один доступный ориентир есть (на
                        // это была проверка if (landmarks.Count > 0))
                        int nearestLandmark = landmarks[0];

                        // Сравним угол ГАММА для этого ориентира с углами ГАММА
                        // для всех остальных доступных ориентиров.
                        //
                        // Проходим по всем остальным доступным ориентирам
                        for (int lm = 1; lm < landmarks.Count; lm++)
                        {
                            // Индекс другого(another) ориентира в списке всех ориентиров
                            int anotherLandmark = landmarks[lm];

                            // Если угол ГАММА для другого ориентира anotherLandmark
                            // меньше, чем для выбранного nearestLandmark
                            if (gamma(xk, yk, x0s[anotherLandmark], y0s[anotherLandmark]) <
                                gamma(xk, yk, x0s[nearestLandmark], y0s[nearestLandmark])
                                )
                            {
                                // Тогда выберем другой ориентир anotherLandmark.
                                nearestLandmark = anotherLandmark;
                            }
                        }

                        // Будет выбран ориентир, по отношению к которому угол ГАММА сейчас
                        // наименьший.
                        selectedLandmark = nearestLandmark;
                    }

                    // Запишем текущий момент времени t и выбранный ориентир
                    // selectedLandmark
                    gaugings[done] = new Gauging(t, selectedLandmark);

                    // После записи измерения в массив, счетчик измерений
                    // инкрементируется
                    done++;
                }


                // Если количество сделанных измерений done равно
                // планируемому count, можно завершать процедуру
                // поиска моментов измерений
                if (done == count)
                {
                    break;
                }


                // Процесс может зацикливаться, если есть такой ориентир, над
                // которым КА не пролетает
                //
                // Введем условие остановки вычислений по времени модели t
                //
                // Положим t равным одним суткам, можно выбрать и другую величину
                //
                // maxTime должно быть не меньше interval * (count - 1) -- наименьшее
                // время, за которое можно выполнить планируемое число измерений, так
                // что при значительном увеличении интервала и количества измерений,
                // maxTime также следует увеличить (изменив следующую строку)
                const double maxTime = 1 * 24 * 60 * 60;

                // Если текущее время (в модели) больше maxTime, то планируемое
                // количество измерений точно не было выполнено до этого момента, иначе
                // бы цикл прервался по условию done == count. Следовательно,
                // планируемое количество измерений за достаточно большой отрезок
                // времени ([0; maxTime]) осуществить нельзя.
                if (t > maxTime)
                {
                    throw new CannotMakeThisManyGaugings();
                }


                // Находим координаты/скорости КА по прошествии интервала измерений
                // (на момент t + interval)
                double[,] nextPosition =
                    RungeCutta.Integrate(derivatives, new double[] { xk, yk, vx, vy }, xs);

                // Обновляем текущие значения координат/скоростей
                // Значения координат/скоростей на последнем шаге интегрирования
                // соответствуют значениям через интервал interval (интервал между
                // измерениями). В массиве nextPosition первый индекс соответствует
                // индексу момента интегрирования, а второй индекс -- индекс уравнения
                // в системе. Так как всего моментов интегрирования interval + 1
                // (моменты интегрирования -- [0, 1, 2, ..., interval]), то индекс
                // последнего измерения равен interval.
                xk = nextPosition[interval, 0];
                yk = nextPosition[interval, 1];
                vx = nextPosition[interval, 2];
                vy = nextPosition[interval, 3];

                // Обновляем текущее время, так как мы вычислили координаты/скорости КА
                // через интервал измерений interval
                t += interval;
            }

            // Возвращаем вычисленные моменты измерений и номера ориентиров, по
            // которым каждое из измерений производится
            return(gaugings);
        }