iCub-main
compute_ekf_fast.m
Go to the documentation of this file.
1 function [xhat,yhat,F,H]=compute_ekf_fast(xhat_1,u_1)
2 %#ok<*NASGU>
3 %
4 % Copyright: (C) 2012 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia
5 % Author: Ugo Pattacini
6 % CopyPolicy: Released under the terms of the GNU GPL v2.0.
7 
8 x1=xhat_1(1);
9 x2=xhat_1(2);
10 x3=xhat_1(3);
11 x4=xhat_1(4);
12 
13 e=exp(-x3/100);
14 
15 xhat=[x1-(x2*(e-1))/x3+((x4*(x3/100+e-1))/x3^2)*u_1;...
16  x2*e+(-(x4*(e-1))/x3)*u_1;...
17  x3;...
18  x4];
19 
21 
22 F=[1, -(e-1)/x3, (x2*(e-1))/x3^2+(x2*e)/(100*x3)-(u_1*x4*(e-1)/100)/x3^2-(2*u_1*x4*(x3/100+e-1))/x3^3, (u_1*(x3/100+e-1))/x3^2;...
23  0, e, (u_1*x4*e)/(100*x3)-(x2*e)/100+(u_1*x4*(e-1))/x3^2, -(u_1*(e-1))/x3;...
24  0, 0, 1, 0;...
25  0, 0, 0, 1];
26 
27 H=[1 0 0 0];
28 
29 
exp(-x3 *T)]
-(x4 *(e-1 u_1()