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
H
H
Definition: compute_ekf_fast.m:27
e
e
Definition: compute_ekf_fast.m:13
x2
x2
Definition: compute_ekf_fast.m:9
u_1
-(x4 *(e-1 u_1()
x3
x3
Definition: compute_ekf_fast.m:10
F
F
Definition: compute_ekf_fast.m:22
xhat
xhat
Definition: compute_ekf_fast.m:15
x4
x4
Definition: compute_ekf_fast.m:11
x1
x1
Definition: compute_ekf_sym.m:28
yhat
yhat
Definition: compute_ekf_fast.m:20
exp
exp(-x3 *T)]