// Функция, по моментам измерения и указанным ориентирам (которые // возвращает предыдущая функция) и заданным начальным условиям // вычисляющая результаты измерений -- углы ФИ в заданный момент // времени по отношению к заданному ориентиру. Иначе говоря, // функция возвращает математичесскую модель вектора измерений 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); }
// Функция, возвращающая моменты времени, в которые выполняются // измерения, и по какому ориентиру измерение каждое измерение // выполняется, при заданных условиях, передаваемых через // параметры: начальных координатах и скоростях КА, координатах // ориентиров, интервале измерений и методе выбора ориентира, // по которому нужно делать измерение, если в данный момент // можно выполнить измерение по нескольким ориентирам. // // Также задается 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); }