[关闭]
@Pigmon 2017-11-25T07:22:04.000000Z 字数 1419 阅读 110

在此处输入标题

未分类


第一问 transitionFunction.m

  1. syms x1 x2 x3
  2. syms u1 u2
  3. syms b_
  4. f1 = x1 + (u1 + u2) / 2 * cos(x3 + (u2 - u1) / (2 * b_));
  5. f2 = x2 + (u1 + u2) / 2 * sin(x3 + (u2 - u1) / (2 * b_));
  6. f3 = x3 + (u2 - u1) / b_;
  7. f_handle = matlabFunction([f1 f2 f3]', 'Vars', {x1 x2 x3 u1 u2 b_});
  8. f = f_handle(x(1), x(2), x(3), u(1), u(2), b);
  9. df = jacobian([f1 f2 f3], [x1, x2, x3]);
  10. Fx_handle = matlabFunction(df, 'Vars', {x1 x2 x3 u1 u2 b_});
  11. F_x = Fx_handle(x(1), x(2), x(3), u(1), u(2), b);
  12. du = jacobian([f1 f2 f3], [u1 u2]);
  13. Fu_handle = matlabFunction(du, 'Vars', {x1 x2 x3 u1 u2 b_});
  14. F_u = Fu_handle(x(1), x(2), x(3), u(1), u(2), b);

运行结果

1.PNG-22.4kB

第二问 measurementFunction.m

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

  1. h = [...
  2. m(1) - x(3)
  3. m(2) - (x(1)*cos(m(1)) + x(2)*sin(m(1)))
  4. ];
  5. H_x = [...
  6. 0, 0, -1
  7. -cos(m(1)), -sin(m(1)), 0
  8. ];
  9. [h(1), h(2), isRNegated] = normalizeLineParameters(h(1), h(2));
  10. if isRNegated
  11. H_x(2, :) = - H_x(2, :);
  12. end

写法 2,变体

  1. syms x1 x2 x3
  2. syms m1 m2
  3. h1 = m1 - x3;
  4. h2 = m2 - (x1 * cos(m1) + x2 * sin(m1));
  5. h_handle = matlabFunction([h1 h2]', 'Vars', {x1 x2 x3 m1 m2});
  6. h = h_handle(x(1), x(2), x(3), m(1), m(2));
  7. H_x = [...
  8. 0, 0, -1
  9. -cos(m(1)), -sin(m(1)), 0
  10. ];
  11. [h(1), h(2), isRNegated] = normalizeLineParameters(h(1), h(2));
  12. if isRNegated
  13. H_x(2, :) = - H_x(2, :);
  14. end

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

  1. syms x1 x2 x3
  2. syms m1 m2
  3. h1 = m1 - x3;
  4. h2 = m2 - (x1 * cos(m1) + x2 * sin(m1));
  5. h_handle = matlabFunction([h1 h2]', 'Vars', {x1 x2 x3 m1 m2});
  6. h = h_handle(x(1), x(2), x(3), m(1), m(2));
  7. dh = jacobian([h1 h2], [x1 x2 x3]);
  8. Hx_Handle = matlabFunction(dh, 'Vars', {x1 x2 x3 m1 m2});
  9. H_x = Hx_Handle(x(1), x(2), x(3), m(1), m(2));
  10. [h(1), h(2), isRNegated] = normalizeLineParameters(h(1), h(2));
  11. if isRNegated
  12. H_x(2, :) = - H_x(2, :);
  13. end

运行结果

  1. measurement function appears to be correct!
添加新批注
在作者公开此批注前,只有你和作者可见。
回复批注