iCub-main
compute_ekf_sym.m
Go to the documentation of this file.
1 function [xhat,yhat,F,H]=compute_ekf_sym(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=sym('x1');
9 x2=sym('x2');
10 x3=sym('x3');
11 x4=sym('x4');
12 T=0.01;
13 
14 Ad=[1 (1-exp(-x3*T))/x3; 0 exp(-x3*T)];
15 Bd=x4*[(exp(-x3*T)+x3*T-1)/(x3*x3); (1-exp(-x3*T))/x3];
16 
17 A_aug=[Ad zeros(2,2); zeros(2,2) eye(2,2)];
18 B_aug=[Bd;0;0];
19 C_aug=[1 0 0 0];
20 
21 x=[x1;x2;x3;x4];
24 
25 F=jacobian(f,x);
26 H=jacobian(h,x);
27 
28 x1=subs(x1,xhat_1(1));
29 x2=subs(x2,xhat_1(2));
30 x3=subs(x3,xhat_1(3));
31 x4=subs(x4,xhat_1(4));
32 
33 xhat=subs(f);
34 yhat=subs(h);
35 F=subs(F);
36 H=subs(H);
37 
38 
B_aug
-(x4 *(e-1 u_1()
xhat
x2
yhat
C_aug
A_aug