roboforum.ru

Технический форум по робототехнике.

 

Тестовый заезд с алгоритмом Калмана

Автомат, адаптивный автомат ... разум

Re: LPS (Локальная Система Позиционирования)

Сообщение =DeaD= » 06 июл 2009, 08:11

Короче будем тестировать так:
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 есть постоянная матрица (опускаем индекс) и является единичной матрицей.
Проект [[Open Robotics]] - Универсальные модули для построения роботов
Аватара пользователя
=DeaD=
 
Сообщения: 24053
Зарегистрирован: 06 окт 2004, 18:01
Откуда: Ебург
прог. языки: Pascal / C++ / PHP / 1C
ФИО: Антон Ботов

Re: LPS (Локальная Система Позиционирования)

Сообщение Duhas » 06 июл 2009, 08:12

Dead, ты с маткадом не знаком? там было бы удобнее мне кажется таки опыты производить...
«Как сердцу выразить себя? … Мысль изреченная есть ложь!»
В этом мире меня подводит доброта и порядочность...
"двое смотрят в лужу, один видит лужу, другой отраженные в ней звезды"
Аватара пользователя
Duhas
 
Сообщения: 6341
Зарегистрирован: 15 сен 2007, 13:03
Откуда: Красноярск
прог. языки: ASM(МК), C(PC)
ФИО: Гагарский Андрей Александрович

Re: LPS (Локальная Система Позиционирования)

Сообщение =DeaD= » 06 июл 2009, 09:16

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 секунды:
Мде, чего-то меня ломает это досчитывать вручную :)
Проект [[Open Robotics]] - Универсальные модули для построения роботов
Аватара пользователя
=DeaD=
 
Сообщения: 24053
Зарегистрирован: 06 окт 2004, 18:01
Откуда: Ебург
прог. языки: Pascal / C++ / PHP / 1C
ФИО: Антон Ботов

Re: Тестовый заезд с алгоритмом Калмана

Сообщение Snable » 06 июл 2009, 13:10

ИМХО, общедоступно - это MATLAB, а не MS Excel. Последнего нет под Линукс, скажем. И, кстати, что в Матлабе, что в Маткаде все было бы наглядней. В Матлабе используется очень простой интерпретирующий язык (почти математический), а в Маткаде - вообще математический язык. Там все записывается формулами (Как формулы из Ворда, скажем, по внешнему виду). Потом эти формулы вместе с результатами и графиками по результатам можно вывести как картинку - удобно, быстро, наглядно. Советую ознакомиться с этими чудесами техники :crazy:
Аватара пользователя
Snable
 
Сообщения: 395
Зарегистрирован: 21 мар 2009, 20:06
Откуда: Город на Неве
прог. языки: GNU C++ with Qt / C# via .NET
ФИО: Александр

Re: Тестовый заезд с алгоритмом Калмана

Сообщение =DeaD= » 06 июл 2009, 13:12

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

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

Добавлено спустя 48 секунд:
Короче будем осваивать инструмент. А есть что-то приличное и бесплатное - опен-сорцное из этой области? Чтобы совсем культурными быть?
Проект [[Open Robotics]] - Универсальные модули для построения роботов
Аватара пользователя
=DeaD=
 
Сообщения: 24053
Зарегистрирован: 06 окт 2004, 18:01
Откуда: Ебург
прог. языки: Pascal / C++ / PHP / 1C
ФИО: Антон Ботов

Re: Тестовый заезд с алгоритмом Калмана

Сообщение Duhas » 06 июл 2009, 13:25

mathcad 2001 лечения вроде большинство дистрибутивов не требует, просто установить и все ) опен соц врядли есть... это довольно серьезный софт )
«Как сердцу выразить себя? … Мысль изреченная есть ложь!»
В этом мире меня подводит доброта и порядочность...
"двое смотрят в лужу, один видит лужу, другой отраженные в ней звезды"
Аватара пользователя
Duhas
 
Сообщения: 6341
Зарегистрирован: 15 сен 2007, 13:03
Откуда: Красноярск
прог. языки: ASM(МК), C(PC)
ФИО: Гагарский Андрей Александрович

Re: Тестовый заезд с алгоритмом Калмана

Сообщение =DeaD= » 06 июл 2009, 13:28

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

Вот с ходу пакеты свободные или полусвободные:
http://www.gnu.org/software/octave/
http://freemat.sourceforge.net/
http://www.r-project.org/
http://www.scilab.org/
Проект [[Open Robotics]] - Универсальные модули для построения роботов
Аватара пользователя
=DeaD=
 
Сообщения: 24053
Зарегистрирован: 06 окт 2004, 18:01
Откуда: Ебург
прог. языки: Pascal / C++ / PHP / 1C
ФИО: Антон Ботов

Re: Тестовый заезд с алгоритмом Калмана

Сообщение Snable » 06 июл 2009, 13:30

Есть SciCad - OpenSource пародия на Matlab с матлабовским же синтаксисом... Но до оригинала ей ооччеенньь далеко... Че-то слышал о языке R - но никогда его не использовал. По описанию - штука мощная. http://ru.wikipedia.org/wiki/R_(язык_программирования)
А OpenOffice как раз таки не понимает OOXML полностью... Точнее, правильнее будет сказать, что он не понимает только Excel-евсие файлы. Так что вот :crazy:
З.Ы.: Чуть-чуть опаздал)))
Аватара пользователя
Snable
 
Сообщения: 395
Зарегистрирован: 21 мар 2009, 20:06
Откуда: Город на Неве
прог. языки: GNU C++ with Qt / C# via .NET
ФИО: Александр

Re: Тестовый заезд с алгоритмом Калмана

Сообщение Duhas » 06 июл 2009, 13:32

ну по крайней мере в маткаде я уверен )))
«Как сердцу выразить себя? … Мысль изреченная есть ложь!»
В этом мире меня подводит доброта и порядочность...
"двое смотрят в лужу, один видит лужу, другой отраженные в ней звезды"
Аватара пользователя
Duhas
 
Сообщения: 6341
Зарегистрирован: 15 сен 2007, 13:03
Откуда: Красноярск
прог. языки: ASM(МК), C(PC)
ФИО: Гагарский Андрей Александрович

Re: Тестовый заезд с алгоритмом Калмана

Сообщение Snable » 06 июл 2009, 13:54

Сейчас прошелся по ссылкам... Октав и фримат оказывается тоже попытки сделать матлаб :crazy: Забавно :crazy:
Вывод: если хочется совместимости и общедоступности - matlab однозначно. Его формат почти все проги (если не все) математические понимают.
Аватара пользователя
Snable
 
Сообщения: 395
Зарегистрирован: 21 мар 2009, 20:06
Откуда: Город на Неве
прог. языки: GNU C++ with Qt / C# via .NET
ФИО: Александр

Re: Тестовый заезд с алгоритмом Калмана

Сообщение =DeaD= » 06 июл 2009, 13:55

Так маткад или матлаб? :)
Проект [[Open Robotics]] - Универсальные модули для построения роботов
Аватара пользователя
=DeaD=
 
Сообщения: 24053
Зарегистрирован: 06 окт 2004, 18:01
Откуда: Ебург
прог. языки: Pascal / C++ / PHP / 1C
ФИО: Антон Ботов

Re: Тестовый заезд с алгоритмом Калмана

Сообщение Snable » 06 июл 2009, 14:25

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

Добавлено спустя 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/
Может быть в вики будет полезно)
Вложения
kalman.png
Выдает такую картинку)))
Аватара пользователя
Snable
 
Сообщения: 395
Зарегистрирован: 21 мар 2009, 20:06
Откуда: Город на Неве
прог. языки: GNU C++ with Qt / C# via .NET
ФИО: Александр

Re: Тестовый заезд с алгоритмом Калмана

Сообщение Duhas » 06 июл 2009, 15:41

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

на маткаде почти любую математику )
«Как сердцу выразить себя? … Мысль изреченная есть ложь!»
В этом мире меня подводит доброта и порядочность...
"двое смотрят в лужу, один видит лужу, другой отраженные в ней звезды"
Аватара пользователя
Duhas
 
Сообщения: 6341
Зарегистрирован: 15 сен 2007, 13:03
Откуда: Красноярск
прог. языки: ASM(МК), C(PC)
ФИО: Гагарский Андрей Александрович

Re: Тестовый заезд с алгоритмом Калмана

Сообщение ems » 12 ноя 2009, 05:34

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

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

Re: Тестовый заезд с алгоритмом Калмана

Сообщение =DeaD= » 12 ноя 2009, 09:16

2ems: Вот если бы вы еще и ТЗ примерно обозначили - было бы вообще круто. Какие погрешности допустимы? ИМС их ведь всегда накапливает?
Проект [[Open Robotics]] - Универсальные модули для построения роботов
Аватара пользователя
=DeaD=
 
Сообщения: 24053
Зарегистрирован: 06 окт 2004, 18:01
Откуда: Ебург
прог. языки: Pascal / C++ / PHP / 1C
ФИО: Антон Ботов

Пред.След.

Вернуться в Алгоритмы

Кто сейчас на конференции

Сейчас этот форум просматривают: нет зарегистрированных пользователей и гости: 1

Mail.ru counter