这是我所谓的“中等大小” EKF的mathlab代码。它使用来自编码器，x和y轴上的加速度计以及陀螺仪z轴上的数据。状态估计向量（8x1）
t：采样时间[s]
P：“先验”估计状态协方差向量（8x8）
z：当前测量向量（5x1）（编码器左；编码器右； x加速度，y加速度，z轴陀螺仪）输出：x：“后验”状态估计矢量（8x1）
P：“后验”状态协方差矢量（8x8）
状态向量x：8x1向量$ \ begin {bmatrix} x \ rightarrow在全局框架中的X位置\\ \ dot x \ rightarrow在X方向上的速度全局框架\\ \ ddot x \ rightarrow的加速度方向全局框架\\ y \ rightarrow Y位置在全局框架\\ \点y \ rightarrow速度在Y方向全局框架\\ \ ddot y \ rightarrow在Y方向全局框架\\ \ theta \ rightarrow的加速度全局框架中的角度\\ \点\ theta \ rightarr ow车辆的角速度\ end {bmatrix} $
测量向量z：
5x1向量$ \ begin {bmatrix} \ eta_ {left} \ rightarrow左轮上的车轮速度脉冲\ \ \ eta_ {right} \ rightarrow右轮的车轮速度脉冲\\ \ dot \ theta_z \ rightarrow陀螺仪MeasurementInZ轴车辆框架\\ a_x \ rightarrow加速度计测量X轴车辆框架\\ a_y \ rightarrow加速度计测量Y轴车辆框架\ end {bmatrix} $
function [x,P] = moodieEKFmedium(x,t,P,z,sigma_ax,sigma_ay,sigma_atau,sigma_odo,sigma_acc,sigma_gyro)
% Check if input matrixes are of correct size
[rows columns] = size(x);
if (rows ~= 8 && columns ~= 1)
error('Input vector size incorrect')
end
[rows columns] = size(z);
if (rows ~= 5 && columns ~= 1)
error('Input data vector size incorrect')
end
% Constants
n0 = 16;
r = 30;
b = 50;
Q = zeros(8,6);
Q(3,3) = sigma_ax;
Q(6,6) = sigma_ay;
Q(8,8) = sigma_atau;
%[Q(1,8),Q(3,6),Q(6,3)] = deal(small);
dfdx = eye(8);
[dfdx(1,2),dfdx(2,3),dfdx(4,5),dfdx(5,6),dfdx(7,8)] = deal(t);
[dfdx(1,3),dfdx(4,6)] = deal((t^2)/2);
dfda = zeros(6,6);
[dfda(3,3),dfda(6,6),dfda(8,8)] = deal(1);
dhdn = eye(5,5);
R = zeros(5,5);
[R(1,1),R(2,2)] = deal(sigma_odo);
R(3,3) = sigma_gyro;
[R(4,4),R(5,5)] = deal(sigma_acc);
%[R(2,1),R(1,2)] = deal(small);
% Predict next state
% xk = f(xk-1)
xtemp = zeros(8,1);
xtemp(1) = x(1) + t*x(2)+((t^2)/2)*x(3);
xtemp(2) = x(2) + t*x(3);
u1 = normrnd(0,sigma_ax);
xtemp(3) = x(3) + u1;
xtemp(4) = x(4) + t*x(5)+((t^2)/2)*x(6);
xtemp(5) = x(5) + t*x(6);
u2 = normrnd(0,sigma_ay);
xtemp(6) = x(6) + u2;
xtemp(7) = x(7) + t*x(8);
u3 = normrnd(0,sigma_atau);
xtemp(8) = x(8) + u3;
x = xtemp
% Predict next state covariance
% Pk = dfdx * Pk-1 * transpose(dfdx) + dfda * Q * transpose(dfda)
P = dfdx * P * transpose(dfdx) + dfda * Q * transpose(dfda);
% Calculate Kalman gain
% Kk = P * transpose(dhdx) [dhdx * P + dhdn * R * transpose(dhdn)]^-1
dhdx = zeros(5,8);
if(x(2) == 0 && x(5) == 0)
[dhdx(1,2),dhdx(2,2)] = deal(0);
[dhdx(1,4),dhdx(2,4)] = deal(0);
else
[dhdx(1,2),dhdx(2,2)] = deal(((t*n0)/(2*pi*r))*(x(2)/sqrt(x(2)^2+x(5)^2)));
[dhdx(1,4),dhdx(2,4)] = deal(((t*n0)/(2*pi*r))*(x(5)/sqrt(x(2)^2+x(5)^2)));
end
%[dhdx(1,2),dhdx(2,2)] = deal(((t*n0)/(2*pi*r))*(x(2)/sqrt(x(2)^2+x(5)^2)));
%[dhdx(1,4),dhdx(2,4)] = deal(((t*n0)/(2*pi*r))*(x(5)/sqrt(x(2)^2+x(5)^2)));
dhdx(1,6) = (t*n0*b)/(2*pi*r);
dhdx(2,6) = -(t*n0*b)/(2*pi*r);
dhdx(4,3) = sin(x(7));
dhdx(4,6) = -cos(x(7));
dhdx(4,7) = (x(3)*cos(x(7)))+(x(6)*sin(x(7)));
dhdx(5,3) = cos(x(7));
dhdx(5,6) = sin(x(7));
dhdx(5,7) = (-x(3)*sin(x(7)))+(x(6)*cos(x(7)));
Kk = P * transpose(dhdx) * (dhdx * P * transpose(dhdx) + dhdn * R * transpose(dhdn))^(-1)
% Update state
H = zeros(5,1);
n1 = normrnd(0,sigma_odo);
H(1) = (((t*n0)/(2*pi*r))*sqrt(x(2)^2+x(4)^2))+(((t*n0*b)/(2*pi*r))*x(6)) + n1;
n2 = normrnd(0,sigma_odo);
H(2) = (((t*n0)/(2*pi*r))*sqrt(x(2)^2+x(4)^2))-(((t*n0*b)/(2*pi*r))*x(6)) + n2;
n3 = normrnd(0,sigma_gyro);
H(3)= x(8) + n3;
n4 = normrnd(0,sigma_acc);
H(4)=(x(3)*sin(x(7))-(x(6)*cos(x(7))))+n4;
n5 = normrnd(0,sigma_acc);
H(5)=(x(3)*cos(x(7))+(x(6)*sin(x(7))))+n5;
x = x + Kk*(z-H)
% Update state covariance
P = (eye(8)-Kk*dhdx)*P;
结束
这是原理图中的滤波器：
这些是状态我使用的转移方程式：
$$ \ x_ {t + 1} = x_ {t} + T \ cdot \ dot x_ {t} + \ frac {T ^ {2}} {2} \ cdot \ ddot x_ {t} $$
$$ \ \ dot x_ {t + 1} = \ dot x_ {t} + T \ cdot \ ddot x_ {t} $$
$$ \ \ ddot x_ {t + 1} = \ ddot x_ {t} + u_ {1} $$
$$ \ y_ {t + 1} = y_ {t} + T \ cdot \ dot y_ {t} + \ frac {T ^ {2}} {2 } \ cdot \ ddot y_ {t} $$
$$ \ \ dot y_ {t + 1} = \ dot y_ {t} + T \ cdot \ ddot y_ {t} $$
$ $ \ \ ddot y_ {t + 1} = \ ddot y_ {t} + u_ {2} $$
$$ \ \ dot \ theta_ {t + 1} = \ dot \ theta_ {t} + T \ cdot \ ddot \ theta_ {t} $$
这些是我使用的观测方程：
$$ \ \ eta_ {left} = \ frac {T \ cdot n_ {0}} {2 \ cdot \ pi \ cdot r} \ cdot \ sqrt {\ dot x ^ {2} + \ dot y ^ {2}} + \ frac {T \ cdot n_ {0} \ cdot b} {2 \ cdot \ pi \ cdot r} \ cdot \ dot \ theta + n_ {1} $$
$$ \ \ eta_ {right} = \ frac {T \ cdot n_ {0}} {2 \ cdot \ pi \ cdot r} \ cdot \ sqrt {\ dot x ^ {2 } + \ dot y ^ {2}}-\ frac {T \ cdot n_ {0} \ cdot b} {2 \ cdot \ pi \ cdot r} \ cdot \ dot \ theta + n_ {2} $$
$$ \ \ dot \ theta_ {z} = \ dot \ theta + n_ {3} $$
$$ \ a_ {x } = \ ddot x \ sin \ theta-\ ddot y \ cos \ theta + n_ {4} $$
$$ \ a_ {y} = \ ddot x \ cos \ theta + \ ddot y \ sin \ theta + n_ {5} $$
小尺寸EKF
我想测试我的过滤器，因此我从一个较小的过滤器开始，在该过滤器中，我只给出了里程表测量值作为输入。这是因为我知道，如果我始终在左右编码器上接收到相同数量的脉冲，那么我的车辆应该行驶直线。
输入：x：“先验”状态估计矢量（6x1）
t：采样时间[s]
P：“先验”估计状态协方差向量（6x6）
z：当前测量向量（2x1）（左侧为编码器；右侧为编码器）输出：x：“后验”状态估计向量（6x1）
P：“后验”状态协方差向量（6x6）
状态向量x：6x1向量$ \ begin {bmatrix} x \ rightarrow在全局框架中的X位置\\ \ dot x \ rightarrow在X方向全局框架中的速度\\ y \ rightarrow在全局框架中的Y位置\\ \点y \ rightarrow在Y方向上的速度全局框架\\ \ theta \ rightarrow车辆角度在全局框架\\ \ dot \ theta \ rightarrow车辆角速度\ end {bmatrix} $
测量向量z：
2x1向量$ \ begin {bmatrix} \ eta_ {left} \ rightarrow左轮的车轮速度脉冲\\ \ eta_ {right} \ rightarrow右轮的车轮速度脉冲\ end {bmatrix} $
% Check if input matrixes are of correct size
[rows columns] = size(x);
if (rows ~= 6 && columns ~= 1)
error('Input vector size incorrect')
end
[rows columns] = size(z);
if (rows ~= 2 && columns ~= 1)
error('Input data vector size incorrect')
end
% Constants
n0 = 16;
r = 30;
b = 50;
Q = zeros(6,6);
Q(2,2) = sigma_ax;
Q(4,4) = sigma_ay;
Q(6,6) = sigma_atau;
%[Q(1,8),Q(3,6),Q(6,3)] = deal(small);
dfdx = eye(6);
[dfdx(1,2),dfdx(3,4),dfdx(5,6)] = deal(t);
dfda = zeros(6,6);
[dfda(2,2),dfda(4,4),dfda(6,6)] = deal(1);
dhdn = eye(2,2);
R = zeros(2,2);
[R(1,1),R(2,2)] = deal(sigma_odo);
%[R(2,1),R(1,2)] = deal(small);
% Predict next state
% xk = f(xk-1)
xtemp = zeros(6,1);
xtemp(1) = x(1) + t*x(2);
u1 = normrnd(0,sigma_ax);
xtemp(2) = x(2) + u1;
xtemp(3) = x(3) + t*x(4);
u2 = normrnd(0,sigma_ay);
xtemp(4) = x(4) + u2;
xtemp(5) = x(5) + t*x(6);
u3 = normrnd(0,sigma_atau);
xtemp(6) = x(6) + u3;
x = xtemp
% Predict next state covariance
% Pk = dfdx * Pk-1 * transpose(dfdx) + dfda * Q * transpose(dfda)
P = dfdx * P * transpose(dfdx) + dfda * Q * transpose(dfda);
% Calculate Kalman gain
% Kk = P * transpose(dhdx) [dhdx * P * transpose(dhdx) + dhdn * R * transpose(dhdn)]^-1
dhdx = zeros(2,6);
if((x(2) < 10^(-6)) && (x(4)< 10^(-6)))
[dhdx(1,2),dhdx(2,2)] = deal((t*n0)/(2*pi*r));
[dhdx(1,4),dhdx(2,4)] = deal((t*n0)/(2*pi*r));
else
[dhdx(1,2),dhdx(2,2)] = deal(((t*n0)/(2*pi*r))*(x(2)/sqrt(x(2)^2+x(4)^2)));
[dhdx(1,4),dhdx(2,4)] = deal(((t*n0)/(2*pi*r))*(x(4)/sqrt(x(2)^2+x(4)^2)));
end
%[dhdx(1,2),dhdx(2,2)] = deal(((t*n0)/(2*pi*r))*(x(2)/sqrt(x(2)^2+x(4)^2)));
%[dhdx(1,4),dhdx(2,4)] = deal(((t*n0)/(2*pi*r))*(x(4)/sqrt(x(2)^2+x(4)^2)));
dhdx(1,6) = (t*n0*b)/(2*pi*r);
dhdx(2,6) = -(t*n0*b)/(2*pi*r);
Kk = P * transpose(dhdx) * ((dhdx * P * transpose(dhdx) + dhdn * R * transpose(dhdn))^(-1))
% Update state
H = zeros(2,1);
n1 = normrnd(0,sigma_odo);
H(1) = (((t*n0)/(2*pi*r))*sqrt(x(2)^2+x(4)^2))+(((t*n0*b)/(2*pi*r))*x(6)) + n1;
n2 = normrnd(0,sigma_odo);
H(2) = (((t*n0)/(2*pi*r))*sqrt(x(2)^2+x(4)^2))-(((t*n0*b)/(2*pi*r))*x(6)) + n2;
x = x + Kk*(z-H)
% Update state covariance
P = (eye(6)-Kk*dhdx)*P;
end
测程观测方程
如果您想知道我如何得出观测方程里程数数据：
<\ _V_ {vl} = V {c} + \ dot \ theta \ cdot b \ rightarrow V_ {vl} = \ sqrt {\ dot x ^ {2} + \ dot y ^ {2}} + \ dot \ theta \ cdot b $ 问题
如果我使用Matlab用户界面尝试使用小型EKF，它似乎确实沿直线行驶，但不要像我期望的那样在0°航向下行驶。即使我从状态向量$ \ x = \ begin {bmatrix} 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \ end {bmatrix} $开始，这意味着从位置[0,0]开始全局坐标系，速度和加速度为零，角度为0°。
在右上角，您可以看到输入的测量数据，即每个采样周期的每个车轮上有5个轮速计数。 （模拟直线行驶的车辆）
在过滤器的一个预测+更新周期结束时，在左上角看到X和Y坐标（来自状态向量）的图形，标有时间周期。
左下角是状态向量中的角度图。您会看到在经过12个周期后，角度仍接近我所期望的0°。
请问有人对这里可能出问题的看法吗？
我一直在思考的解决方案
我可以使用这个问题中解释的“里程运动模型”。不同之处在于，里程计数据已插入到过滤器的预测步骤中。但是，如果这样做，我会看到2个问题：1）我看不到如何为测试目的制作小型版本，因为我不知道在更新步骤中要添加哪些度量，以及2 ）对于中等大小的版本，我不知道如何制作观察方程，因为状态矢量并不意味着速度和加速度。
我可以使用“里程表运动模型”，而在更新步骤中使用欧拉角，可链接到$ \ \ theta $。我可以从IMU中实现的数字运动处理器（DMP）中获得该欧拉角。这样，角速度不在状态矩阵中就没有问题了。但是比起我，加速度观测方程仍然有问题。
#1 楼
除此之外，我最喜欢的调试行为异常的过滤器的方法是隔离每个步骤。您的机器人应以0,0,0作为开始状态和恒定的vel，向右直行。否则，您的测量将校正模型，而不是传感器噪声。删除测量步骤并手动进料残渣。零向量残差应产生与开始时相同的状态（也非常适合验证状态转换，如上所述可能是错误的）。如果零残差产生非零变化，则您的K矩阵通常是错误的，并且通常是因为状态转换Jacobian很不稳定。
如果预测和残差有意义，那么也要分别检查测量值。向其中输入状态和Odom测量值，以确保产生的残差与您的期望相符。例如。 1,1直行，2,1右弯，1,2左，1，-1仅调整航向等。或显示它，然后出现管道问题或显示问题。由于显示错误，我浪费了数小时重新定义所有方程式。
在您的情况下，我不清楚里程表方程式。您似乎正在控制x，y，theta中的vel，但是使用里程表来更新该速度。您实际上不是在发出车轮速度命令吗？
#2 楼
您应该先验证过滤器是否工作，然后再猜测建模选择。但是我同意这两个过滤器看起来都不错（尽管我没有仔细检查所有数学方法），而且您建议的两个更改也都可以使用。我曾经使用过一个过程来发现KF中的错误，但它绝不是全面的：
它们应该从大开始并渐近地减小到本底噪声。
您的残差应该落在门内，因为通常的高斯容差间隔（68、95和99.7）％的残差落在（ $ 1 \ sigma $，$ 2 \ sigma $，$ 3 \ sigma $）。
如果不是这种情况，则说明您的过滤器无法正常运行，这通常是由于实际KF实现或测量模型中的错误造成的。
如果正在收敛，但质量比预期的差，则可能只是协方差调整问题
验证测量模型并通过禁用预测阶段来更新步骤
对于每个预测步骤，设置$ x_ {k} = x_ {k-1} $但继续增长$ P $
如果残差没有减少，则您的测量模型可能是错误的（因为您的模型可观察到的角度和位置）
再次启用您的Predict阶段，现在以相同的方式验证您的过程模型
有关一组全面的讲义，请参阅ACFR的KF教程，特别是在幻灯片121之后。
评论
您链接的里程表解决方案背后的假设是，里程表脉冲不是状态的度量，而是控制输入的度量。这是一种设计选择。我很惊讶您在测试之前对整个过滤器进行了编码。