С самим фильтром разобрался, а вот модель не могу слепить... Есть 1-осевой акселерометр, надо получить скорость и дистанцию. Задача более теоретическая, чем практическая.
Отбросим все шумы.
Код
x[k] = F*x[k-1] + B*u[k]; - Модель реального состояния
z[k] = H*x[k]; - Модель измеренного состояния
Вктор состояния x=[real_acceleration; real_velocity; real_position];
z[k] = H*x[k]; - Модель измеренного состояния
Вктор состояния x=[real_acceleration; real_velocity; real_position];
Вектор воздействия по идее должен быть u=[real_acceleration; 0; 0];, верно?
Тогда матрицы F, B, H по идее должны быть такими:
Код
F=[
0 0 0
Ts 1 0
0 Ts 1 ];
B=H=[
1 0 0
0 0 0
0 0 0 ];
0 0 0
Ts 1 0
0 Ts 1 ];
B=H=[
1 0 0
0 0 0
0 0 0 ];
Теперь вот в чем запутался - вкторы z и u, которые собственно подаем на фильтр калмана. в какой из них пихать данные с акселерометра? Ведь, если пихать в u, а z обнулить - не будет работать корректор. Если пихать в z, а u обнулить - не будет работать эстиматор? Или я чего-то не понял до конца?
Напрашивается только приведение матрицы F до такого вида, приняв, что ускорение не меняется:
Код
F=[
1 0 0
Ts 1 0
0 Ts 1 ];
1 0 0
Ts 1 0
0 Ts 1 ];
Тогда обнулив u, B и установив вектор z=[measured_acc; 0; 0]; вроде все логично получается, если я все правильно понял...
пс. Имена переменных такие, как тут http://en.wikipedia.org/wiki/Kalman_Filter#The_Kalman_filter
