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