vehicle_noise.m

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

function [x_new_noise] = vehicle_noise(delta,x_old)

% output: x_new_noise = noisy,reel 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
% error due to odometry 

global odomtrie_var

% model 2
delta_v    = delta(1)+randn(1).*sqrt(odomtrie_var(1))
delta_w    = delta(2)+randn(1).*sqrt(odomtrie_var(2))
x_new_noise = x_old + [cos(x_old(3));sin(x_old(3));0].*delta_v +[0;0;1].*delta_w;