математика фильтра Калмана., как пересчитывать веса в ковариционной матрице Опции

[Serj78]

Требуется построить фильтр калмана для 2-х величин. это курс от ИНС и от GPS.
накачал некоторое количество информации, и понял, что подход Бесерского почему-то другой, нежели у буржуев ( в Навгеокоме статья, если надо, выложу.) у буржуев отсутствует формирующий фильтр, что по Бесекерскому есть модель нашего процесса. Пересчет весов вклада компонент вектора измерений как я понял, на каждом шаге вычисляется их дисперсий предыдущих оценок выходного вектора оценки состояния. мне пока не ясно КАК ИМЕННО МАТЕМАТИЧЕСКИ вычислять дисперсии и веса. если кто занимался этим, помогите, пожалуйста.

[neptune-1]

Требуется построить фильтр калмана для 2-х величин. это курс от ИНС и от GPS.
....
цель построения калмановского фильтра- улучшить динамические характериски автопилота ДПЛА и попытаться прогнозировать отставание курса GPS при больших угловых скоростях - более 35 °/c

Запишем формулы для структуры измерений:

Ygps = PSI - L*PSIdot + ngps;
Yins = PSI + EPS + nins; (1)

PSI - курс, PSIdot - скорость изменения курса, L - коэффициент который отвечает за "отставание курса GPS при больших угловых скоростях", EPS - дрейф ИНС; ngps, nins - шумы измерений, к которым (с натягом можно отнести неучтенные ошибки). Дисперсия nins будет Nins~=(0.001 °/c)^2 (потом можно подобрать вручную). Дисперсия ngps будет Ngps=(рассчитывается на лету, зависит от горизонтальной скорости)

Введем вектор состояний:
X = [ PSI PSIdot L EPS EPSdot]; (2)

EPSdot - скорость дрейфа курса от ИНС.

Введем вектор измерений

Y = [Ygps; Yins]; (3)

Введем векторную сигнальную функцию на основании (1), (3):

S(X) = [PSI - L*PSIdot;
PSI + EPS]; (4)

N=[Nins 0;
0 Ngps]; - матрица дисперсий измерений.

Записываем переходную матрицу для вектора состояний:

F = [1 T 0 0 0;
0 1 0 0 0;
0 0 1 0 0;
0 0 0 1 T;
0 0 0 0 1]; (5)
T - шаг дискретизации по времени, сек.

Т.е.
X(k) = F*X(k-1) + g(k)(вектор формирующих шумов);
- аппроксимация вектора состояний Марковским процессом (чисто для справки).
g = [0 ksiPSIdot ksiL 0 ksiEPS];

ковариационная матрица g равна

G=diag([0; sigmaPSIdot^2; sigmaL^2 0 sigmaEPS^2]); (6)

sigmaPSIdot = T*(среднее угловое ускорение);
sigmaL = (что-то очень маленькое для вычислительной стабильности и демпфирования погрешностей, связанных с гауссовским приближением апостериорной плотности вероятности X);
sigmaEPS = T*(4e-6 рад/с/с - для микромеханической БИНС; 1е-9 рад/с/с для ИНС класса Litton LN-200)

Задаем начальные условия:

X(0) = [Ygps(0); 0; 0; 0; 0];
E(0) = diag([Ngps(0); 100; 1; 0.1; 0.001]); - ковариационная матрица оценок, или матрица дисперсий вектора сосотояний (кому как больше нравится).

Запускаем обсчет расширенного фильтра Калмана. На каждом шаге просчитываются 4 строчки:

Экстраполяция:
Eext(k) = F*E(k-1)*F + G; (7)
Xext(k) = F*X(k-1); (8)

Оценивание (учет измерений):
E(k) = inv(Eext(k) + (dS(Xext)/dX)'*inv(N(k))*(dS(Xext)/dX)); (9)
X(k) = Xest(k) + E(k)*(dS(Xext)/dX)'*inv( N(k) )*( Y(k) - S(Xext(k)) ); (10)

X(k) - и есть оценка вектора состояний, а его первый компонент (PSI) - оптимальная оценка курсового угла. Третий компонент (L) - коэффициент запаздывания курсовых измерений из GPS.

Задача решена.

Пояснения:

dS(Xext)/dX - это МАТРИЦА 2х5, производная сигнальной функции по вектору состояний. Считается на каждом шаге для экстраполированных значений X=Xext(k). В данном cлучае:

(dS(X)/dX) = [1 -L -PSIdot 0 0;
1 0 0 1 0]; (11)

Если темп GPS меньше темпа ФК, то для тех моментов времени, когда данные GPS недоступны полагается

inv( N(k) ) = [0 0; 0 (1/Nins)]; (12)


Желаю удачи.