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

[Serj78]

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

[pdk]

От исходников бы не отказался

Вот еще интересный проект: Исходники Кальмана для Motion Tracking

[wookX]

От исходников бы не отказался

Вот пожалуйста исходники от книги описанной выше.
Может у кого нибудь есть исходники от книжки Global Positioning System, Inertial navigation and Integration?

асколько я понял в моем случае надо воспользоваться Многомерным цифровым фильтром Калмана.
Для этого мне нужно получить:
1) Модель сообщения
2) Модель Наблюдения
3) Априорные данные
4) Уравнение Фильтра Калмана
5) Коеффициент Усиления
6) Апостериорная Матрица Дисперсий
7) Начальные условия

Но вот как это все применить для моего случая я не знаю

Прикрепленные файлы

 Kalman_Filtering___Theory_and_Practice_using_MATLAB__2nd_Ed.zip ( 36.41 килобайт ) Кол-во скачиваний: 279

 

[wookX]

Алгоритм получается следующий:
[font=Courier New]

Код

% Accelerometer state vector
  Xa=[ba S1 d12 S2 d13 d23 S3 (FX1-FI1)]';

% State transition matrix
  Fa=I

% Unknown accelerometer biases, normalized to the magnitude of gravity
  ba=[ba1;
    ba2;
    ba3];

% Unknown acceleration-squared nonlinearity for acceleration along the acelerometer input axis
  FI1=[FI11;
     FI12;
     FI13];

% Unknown acceleration-squared nonlinearity for acceleration normal to the acelerometer input axis
  FX1=[FX11;
     FX12;
     FX13];

% Unknown accelerometer scale factor errors
  S1 S2 S3

% Unknown accelerometer axes nonorthogonalities
  d12 d13 d23

% Midpoint components of acceleration in platform coordinates
  bm^2=[b1^2 0  0;
    0  b2^2 0;
    0  0  b3^2];

  bm=[b1 b2 b3]';

Ha=[b1,b3,b1^2,b1,b2,b1,b3,b2^2,b2,b3,b3^2,(1-b1^2)*b1,(1-b2^2)*b2,(1-b3^2)*b3];

%%%%%%%%%%%%%%
% EKF module %
%%%%%%%%%%%%%%
% Input initial parameters Rk, xa, Za, Ha, FY
% xa=[0 0 0 0 0 0 0 0 0 0 0 0]
Id=[1 0 0 0 0 0 0 0 0 0 0 0;
    0 1 0 0 0 0 0 0 0 0 0 0;
    0 0 1 0 0 0 0 0 0 0 0 0;
    0 0 0 1 0 0 0 0 0 0 0 0;
    0 0 0 0 1 0 0 0 0 0 0 0;
    0 0 0 0 0 1 0 0 0 0 0 0;
    0 0 0 0 0 0 1 0 0 0 0 0;
    0 0 0 0 0 0 0 1 0 0 0 0;
    0 0 0 0 0 0 0 0 1 0 0 0;
    0 0 0 0 0 0 0 0 0 1 0 0;
    0 0 0 0 0 0 0 0 0 0 1 0;
    0 0 0 0 0 0 0 0 0 0 0 1];
% Computing Kalman gain
  Kg = Pk*Ha'*inv(Ha*Pk*Ha'+Rk); %Kg = (Pk*Hk')/(Hk*Pk*Hk'+Rk);

% Computing the predicted measurment
  za = Ha*xa;

% Conditioning the predicted estimate on the measurement
  Xa = xa + Kg*(Za - za);

% Computing a posteriori covariance matrix for update estimate
  P2k= (Id-Kg*Ha)*Pk;

% Computing the a priori covariance matrix ahead for next time step
  Pk = FY*P2k*FY' + Qk;

% Computing the predicted state estimate
  xaf = (FY*Xa)';

Ничего не забыл?
кто разбирается может подскажет а? Нигде не напортачил???

[CodeWarrior1241]

Ничего не забыл?
кто разбирается может подскажет а? Нигде не напортачил???

Difference Equation надо давать?

[wookX]

Пока запустить не удалось (
Нашлось несколько ошибочек.
Должно быть вот так:

Код

% Accelerometer state vector
  Xa=[ba' S1 d12 S2 d13 d23 S3 (FX1-FI1)']';

и вот так вот

Код

Ha=[b1,b2,b3,b1^2,b1*b2,b1*b3,b2^2,b2*b3,b3^2,(1-b1^2)*b1,(1-b2^2)*b2,(1-b3^2)*b3]';

Код

%Difference equation for the accelerometers
Xa(i)=Fa*Xa(i-1)+wa(i-1)

Код

%Accelerometer observation equation
Za(i)=Ha*Xa(i)+Va(i)

А как UKF построить?

[pdk]

А как UKF построить?

Вот две статьи:
"A New Extension of the Kalman Filter to Nonlinear Systems" Simon J. Julier K. Uhlmann
Введение. Информации достаточно для реализации.

"THE SQUARE-ROOT UNSCENTED KALMAN FILTER FOR STATE AND PARAMETER-ESTIMATION" Rudolph van der Merwe and Eric A. Wan
Более эффективная реализация. ( QR + Холецкий )
Похоже что в коде по приведенной выше ссылке именно эта реализация алгоритма.

Прикрепленные файлы

 Julier1997_SPIE_KF.pdf ( 305.63 килобайт ) Кол-во скачиваний: 422
 merwe01a.pdf ( 166.49 килобайт ) Кол-во скачиваний: 341