vehicle.m

扩展卡尔曼滤波算法是滤波领域较为重要的方法之一。本滤波算法是典型的卡尔曼滤波应用问题。

function [x_new] = vehicle(delta,x_old)

% output: x_new = desired state of the vehicle [x,y,w] 
% input : delta [delta_v,delta_w] for non-holonomic-robot | x_old =
% x_new_real one time step before

delta_v    = delta(1);
delta_w    = delta(2);
x_new      = x_old + [cos(x_old(3));sin(x_old(3));0].*delta_v +[0;0;1].*delta_w;