-
Notifications
You must be signed in to change notification settings - Fork 10
/
move.m
45 lines (37 loc) · 737 Bytes
/
move.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
%运动模型
%角度每次只增加固定输入和噪声
%下一时刻会到达当前局部坐标系下的(dx,0)点
function [ro, RO_r, RO_n] = move(r, u, n, dt)
% dt is not used by this function.
a = r(3);
dx = u(1) + n(1);
da = u(2) + n(2);
ao = a + da;
dp = [dx;0];
%统一调到[-pi,pi]
if ao > pi
ao = ao - 2*pi;
end
if ao < -pi
ao = ao + 2*pi;
end
if nargout == 1
to = fromFrame2D(r, dp);
else
[to, TO_r, TO_dp] = fromFrame2D(r, dp);
AO_a = 1;
AO_da = 1;
RO_r = [TO_r ; 0 0 AO_a];
RO_n = [TO_dp(:,1) zeros(2,1) ; 0 AO_da];
end
ro = [to;ao];
end
function f()
%%
syms x y a dx da real
X = [x;y;a];
u = [dx;da];
[xo, XO_x, XO_u] = move(X, u, zeros(2,1));
simplify(XO_x - jacobian(xo,X))
simplify(XO_u - jacobian(xo,u))
end