roboforum.ru

Короче будем тестировать так:
1. Положение и скорость будем брать из реальной траектории Pos(t)=10*sin(t/15). При этом t будем брать от 1 до 40. Скорость реальная очевидно Speed(t)=10/15*cos(t/15).
2. Погрешность положения возьмём тупо (рандом()*3-1.5), погрешность скорости (рандом()/4-0.125)

Добавлено спустя 3 минуты 55 секунд:
3. Управляющего воздействия нет, поэтому матрица Bk и вектор uk нулевые.
4. Исходя из pos(k)=pos(k-1)+speed(k-1)*dt получаем, что матрица прогнозного состояния системы Fk будет фиксирована (значит опускаем индекс k), и будет равна:

(1 1)
(0 1)

5. Так как мы наблюдаем датчиками прямо значения переменных описывающих состояние системы - матрица Hk есть постоянная матрица (опускаем индекс) и является единичной матрицей.

Dead, ты с маткадом не знаком? там было бы удобнее мне кажется таки опыты производить...

6. Осталось определить матрицы ковариаций Q и R.

Так как мы знаем, что система достаточно стабильна, примем Q равной

(0.25 0)
(0 0.025)

Далее определим кватраты матожидания отклонений наблюдений от реальных значений - матрицу R:

(0.75 0)
(0 0.0625)

PS 2duhas: Не знаком, да и хотелось общедоступно - а это однозначно эксель

Добавлено спустя 12 минут 23 секунды:
Вектор системы будем писать (x1(k), x2(k)),

Матрицу ковариаций системы будем писать как:
(Pa(k), Pb(k))
(Pc(k), Pd(k))

Добавлено спустя 9 минут 15 секунд:
Шаг предсказания (припишем ко всем значениям '):

x'(k)=F*x(k-1):

x1'(k)=x1(k-1)+x2(k-1)
x2'(k)=x2(k-1)

P'(k)=F*P(k-1)*FT+Q: (где FT - F транспонированная)

Код: Выделить всё

( Pa'(k) Pb'(k) ) = (1 1) * (Pa(k-1) Pb(k-1)) * (1 0) + (0.25 0)
( Pc'(k) Pd'(k) )   (0 1)   (Pc(k-1) Pd(k-1))   (1 1)   (0 0.025)

Добавлено спустя 6 минут 56 секунд:
=>

Код: Выделить всё

( Pa'(k) Pb'(k) ) = (1 1) * (Pa(k-1)+Pb(k-1) Pb(k-1))  + (0.25 0)
( Pc'(k) Pd'(k) )   (0 1)   (Pc(k-1)+Pd(k-1) Pd(k-1))    (0 0.025)

=>

Код: Выделить всё

( Pa'(k) Pb'(k) ) = (Pa(k-1)+Pb(k-1)+Pc(k-1)+Pd(k-1) Pb(k-1)+Pd(k-1)) + (0.25 0)
( Pc'(k) Pd'(k) )   (Pc(k-1)+Pd(k-1)                 Pd(k-1))           (0 0.025)

=>

Код: Выделить всё

( Pa'(k) Pb'(k) ) = (Pa(k-1)+Pb(k-1)+Pc(k-1)+Pd(k-1)+0.25 Pb(k-1)+Pd(k-1))
( Pc'(k) Pd'(k) )   (Pc(k-1)+Pd(k-1)                      Pd(k-1)+0.025)

=>

Pa'(k)=Pa(k-1)+Pb(k-1)+Pc(k-1)+Pd(k-1)+0.25
Pb'(k)=Pb(k-1)+Pd(k-1)
Pc'(k)=Pc(k-1)+Pd(k-1)
Pd'(k)=Pd(k-1)+0.025

Добавлено спустя 1 минуту 51 секунду:
Вектор наблюдения (z1(k), z2(k))

Отклонение наблюдения от ожидаемого (т.к. матрица H единичная)

Код: Выделить всё

(y1(k)) = (z1(k)) - (x1(k))
(y2(k))   (z2(k))   (x2(k))

Добавлено спустя 4 минуты 5 секунд:
Матрица ковариаций отклонения (т.к. H единичная):
S(k)=P'(k)+R

K(k)=P'(k)/S(k)

x(k)=x'(k)+K(k)*y(k)

P(k)=(I-K(k))*P'(k)

Добавлено спустя 22 минуты 14 секунд:
Sa(k)=Pa'(k)+0.75
Sb(k)=Pb'(k)
Sc(k)=Pc'(k)
Sd(k)=Pd'(k)+0.0625

W(k)=1/S(k)

Код: Выделить всё

(Wa(k) Wb(k))=1/d(k)*( Sd(k) -Sb(k))
(Wc(k) Wd(k))        (-Sc(k)  Sa(k))

где d(k)=d(S(k))=Sa(k)*Sd(k)-Sb(k)*Sc(k)

Добавлено спустя 3 минуты 34 секунды:
Мде, чего-то меня ломает это досчитывать вручную

Вообще в линухе есть OpenOffice, который замечательно понимает Excel

Но мысль про маткад я понял

Добавлено спустя 48 секунд:
Короче будем осваивать инструмент. А есть что-то приличное и бесплатное - опен-сорцное из этой области? Чтобы совсем культурными быть?

mathcad 2001 лечения вроде большинство дистрибутивов не требует, просто установить и все ) опен соц врядли есть... это довольно серьезный софт )

Да есть еще как опен-сорц и даже не один, это же образовательный софт в существенной части, вопрос в том, насколько какой распространен и какой легче в освоении.

Вот с ходу пакеты свободные или полусвободные:
http://www.gnu.org/software/octave/
http://freemat.sourceforge.net/
http://www.r-project.org/
http://www.scilab.org/

Есть SciCad - OpenSource пародия на Matlab с матлабовским же синтаксисом... Но до оригинала ей ооччеенньь далеко... Че-то слышал о языке R - но никогда его не использовал. По описанию - штука мощная. http://ru.wikipedia.org/wiki/R_(язык_программирования)
А OpenOffice как раз таки не понимает OOXML полностью... Точнее, правильнее будет сказать, что он не понимает только Excel-евсие файлы. Так что вот
З.Ы.: Чуть-чуть опаздал)))

ну по крайней мере в маткаде я уверен )))

Сейчас прошелся по ссылкам... Октав и фримат оказывается тоже попытки сделать матлаб Забавно
Вывод: если хочется совместимости и общедоступности - matlab однозначно. Его формат почти все проги (если не все) математические понимают.

Так маткад или матлаб?

Это на вкус и цвет. Маткад по сравнению с матлабом - "калькулятор для детей дошкольного возраста". У Матлаба есть куча справочной информации, огромное сообщество "просвященных", куча портов и привязок на все что можно, куча функций и тулбоксов на все случае жизни. Возможность компилять исполняемый модуль, возможность писать графический интерфейс, возможность писать сценарии(программы) так, что они будут все, скажем в Вордовский файл выводить с разметкой, с формулами и так далее. Возможно даже интегрировать их. То есть пишешь формулу в ворде, клацаешь какую-то комбинацию (уже забыл какую) - он тебе её считает и все графики прямо в документе перестраивает. Или интегрировать в Эксель тот-же. Тогда можно использовать почти любую матлабовскую функцию из экселя (а это может быть и скрипт) со всеми вытекающими... В общем плюсов куча...
У Маткада перед матлабом есть только один плюс, ито спорный - то что там формулы вводяться в более понятном человеку виде. Лично меня это бесит(когда вводишь), но на вкус и цвет - фломастеры разные
ИМХО, Матлаб все-таки лучше будет. Кстати, много раз видел в нэте, что роботов через матлаб управляют... или данные с осциллографоф анализируют через матлаб... А вот с маткадом ни разу таких примеров не видел. Значит народ все-таки матлаб в основном использует.

Добавлено спустя 14 минут 55 секунд:
Кстати, в Матлабе есть даже тулбокс для фильтров Кальмана:
http://www.cs.ubc.ca/~murphyk/Software/ ... alman.html
Есть туториалы с комментами:
http://www.mathworks.com/matlabcentral/ ... hange/5377
http://www.mathworks.com/matlabcentral/ ... ange/18465
http://www.mathworks.com/matlabcentral/ ... ange/18189
http://www.mathworks.com/matlabcentral/ ... ange/18217
http://www.mathworks.com/matlabcentral/ ... ange/12307
И еще куча других...
Вот, например, код:

Код: Выделить всё

% Extended Kalman Filter Demo

%{

Version 1.0, September 2006

This tutorial was written by Jose Manuel Rodriguez,
Universidad Politecnica de Cataluсa, Spain
Based on the Technical Document by
Juan Andrade-Cetto, "The Kalman Filter", March 2002
%}

%{
x is the real plant behavior, in this case a sinus wave
with the following formulation:

x(time)=sin(frec(@ time-1)*time-1) + ramdom error signal("sigmav")
frec(time)=frec(@ time-1)

x_ is the predicted state, this is where Kalman filter will come
and where we will correct our estimations using an observation

z is the observation of the real plant, in this case corresponding only to
the position at a given time. Note that this observation is subject to an
error, therefore the resulting equation is: z(time)=x(time)+ramdom error("sigmaw")

Our first prediction will come from the plant ideal behavior, then
using the observations and error covariance we will obtain a better estimate.

xc is the ideal plant behavior... this is used just for comparison

P is the state error covariances at a given time of all the involved variables,
note that we are forming x as a 2 by 1 matrix with the following:
x(1,n) -> position
x(2,n) -> frecuency

Our functions are as following for this example:
Let's say:
f1: x1(time)=sin(x2(time-1)*(time-1))+V     V->ramdom plant error
f2: x2(time)=x2(time-1)
h:  y=x1+w                                  w->ramdom sensor error

F is the Jacobian of the transfer function due to the involved variables,
in this case these are x1 and x2, therefore F will be a 2 by 2 matrix
(always the matrix is square). The resulting F depends on time and must be
computed for every step that the system takes.

F is as follows:
F -> df1/dx1 = 0    df1/dx2 = cos(x2*time)*time
     df2/dx1 = 0    df2/dx1 = 1

%}

clear all; close all;


% Initial Conditions
x(:,1) = [0;0.05];      %Our real plant initial condition       
x_(:,1) = [0;0.04];     %Our estimate initial conidition (they might differ)
xc = x_;                %Set the ideal model as we think it should start
P = [0.01 0;            %set initial error covariance for position & frec, both at sigma 0.1, P=diag([sigma_pos_init^2 sigmav_frec_init^2])
     0     0.01];
sigmav = 0.1;           %the covariance coeficient for the position error, sigma
sigmaw = 0.5;           %the covariance coeficient for the frecuency error, sigma
Q = sigmav*sigmav;      %the error covariance constant to be used, in this case just a escalar unit
R = sigmaw*sigmaw;      %the error covariance constant to be used, in this case just a escalar unit

G = [1;0];              %G is the Jacobian of the plant tranfer functions due to the error.
H = [ 1 0];             %H is the Jacobian of the sensor transfer functions due to the variables involved
W = 1;                  %W is the Jacobian of the sensor transfer functions due to the error.

steps = 1000;   %Amount of steps to simulate

% bucle
for i =2:steps          %start @ time=2
  % the real plant
  x(:,i) = [sin(x(2,i-1)*(i-1)) + randn*sigmav ; x(2,i-1) ];
  z(i) = x(1,i) + randn*sigmaw;

  % blind prediction (just for comparison)
  xc(:,i) = [sin(xc(2,i-1)*(i-1)); xc(2,i-1)]; 
  % prediction
  x_(:,i) = [sin(x_(2,i-1)*(i-1)); x_(2,i-1)];
  z_(i) = x_(1,i);

  % compute F
  F = [0 i*cos(x_(2,i)*i);
       0 1];
 
  % Prediction of the plant covariance
  P = F*P*F' + G*Q*G';
  % Innovation Covariance
  S = H*P*H'+R;
  % Kalman's gain
  K = P*H'*inv(S);
  % State check up and update
  x_(:,i) = x_(:,i) + K * (z(i)-z_(i));
 
  % Covariance check up and update
  P = (eye(2)-K*H)*P;
 
  sigmaP(:,i)=sqrt(diag(P)); %sigmap is for storing the current error covariance for ploting pourposes
end

figure(1);clf; hold on;
plot(x(1,:),'-b');                  %plot the real plant behavior
plot(z,'.r');                       %plot the observations over this plant
plot(x_(1,:),'-g');                 %plot the Kalman filter prediction over the plant
plot(xc(1,:),'-m');                 %The original thought of the plant
plot(x_(1,:)+2*sigmaP(1,:),'-g');   %These two are the threshold in witch I'm certain that the plant state is at a given time
plot(x_(1,:)-2*sigmaP(1,:),'-g');


figure(2);clf;hold on;
plot(x(2,:),'-b');                  %Frecuency estimation
plot(x_(2,:),'-g');                 %Frecuency filtered by Kalman
 

Вот еще че нарыл:
http://www.nsu.ru/matlab/Exponenta_RU/e ... z6.asp.htm
и вот:
http://www.basegroup.ru/library/cleaning/kalmanfilter/
Может быть в вики будет полезно)

на матлабе можно сделать почти все...

на маткаде почти любую математику )

Здоровья всем, уважаемые.

У нас стоит задача, которая здесь обсуждается - необходимо под инерциалку написать прогу для локального позиционирования. Понимаю, что предстоит много трудностей с двойным интегрированием. Думали использовать готовые IMU от SparkFun http://www.sparkfun.com/commerce/product_info.php?products_id=8454, но порывшись на форумах - уже и сомневаемся. Подскажите, получился ли положительный результат по теме. Не хотелось бы 500$ на воздух потратить.

2ems: Вот если бы вы еще и ТЗ примерно обозначили - было бы вообще круто. Какие погрешности допустимы? ИМС их ведь всегда накапливает?