在此处输入标题

未分类

第一问 transitionFunction.m

syms x1 x2 x3
syms u1 u2
syms b_
f1 = x1 + (u1 + u2) / 2 * cos(x3 + (u2 - u1) / (2 * b_));
f2 = x2 + (u1 + u2) / 2 * sin(x3 + (u2 - u1) / (2 * b_));
f3 = x3 + (u2 - u1) / b_;
f_handle = matlabFunction([f1 f2 f3]', 'Vars', {x1 x2 x3 u1 u2 b_});
f = f_handle(x(1), x(2), x(3), u(1), u(2), b);
df = jacobian([f1 f2 f3], [x1, x2, x3]);
Fx_handle = matlabFunction(df, 'Vars', {x1 x2 x3 u1 u2 b_});
F_x = Fx_handle(x(1), x(2), x(3), u(1), u(2), b);
du = jacobian([f1 f2 f3], [u1 u2]);
Fu_handle = matlabFunction(du, 'Vars', {x1 x2 x3 u1 u2 b_});
F_u = Fu_handle(x(1), x(2), x(3), u(1), u(2), b);

运行结果

第二问 measurementFunction.m

写法 1，鲁小伟网上找到的标准答案

h = [...
    m(1) - x(3)
    m(2) - (x(1)*cos(m(1)) + x(2)*sin(m(1)))
    ];
H_x = [...
    0,          0,          -1
    -cos(m(1)), -sin(m(1)),  0
    ];
[h(1), h(2), isRNegated] = normalizeLineParameters(h(1), h(2));
if isRNegated 
    H_x(2, :) = - H_x(2, :);
end

写法 2，变体

syms x1 x2 x3
syms m1 m2
h1 = m1 - x3;
h2 = m2 - (x1 * cos(m1) + x2 * sin(m1));
h_handle = matlabFunction([h1 h2]', 'Vars', {x1 x2 x3 m1 m2});
h = h_handle(x(1), x(2), x(3), m(1), m(2));
H_x = [...
    0,          0,          -1
    -cos(m(1)), -sin(m(1)),  0
    ];
[h(1), h(2), isRNegated] = normalizeLineParameters(h(1), h(2));
if isRNegated 
    H_x(2, :) = - H_x(2, :);
end

写法 3，我写的不标准答案

syms x1 x2 x3
syms m1 m2
h1 = m1 - x3;
h2 = m2 - (x1 * cos(m1) + x2 * sin(m1));
h_handle = matlabFunction([h1 h2]', 'Vars', {x1 x2 x3 m1 m2});
h = h_handle(x(1), x(2), x(3), m(1), m(2));
dh = jacobian([h1 h2], [x1 x2 x3]);
Hx_Handle = matlabFunction(dh, 'Vars', {x1 x2 x3 m1 m2});
H_x = Hx_Handle(x(1), x(2), x(3), m(1), m(2));
[h(1), h(2), isRNegated] = normalizeLineParameters(h(1), h(2));
if isRNegated 
    H_x(2, :) = - H_x(2, :);
end

运行结果

measurement function appears to be correct!