@Pigmon 2017-11-25T07:22:04.000000Z 字数 1419 阅读 157

# 在此处输入标题

未分类

## 第一问 transitionFunction.m

syms x1 x2 x3syms u1 u2syms 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 x3syms m1 m2h1 = 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 x3syms m1 m2h1 = 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!

• 私有
• 公开
• 删除