ジャイロスコープと加速度計のデータを融合するための四元数拡張カルマン フィルターを設計しました。推定プロットの形状は完璧に見えますが、推定は常に間違った解に収束しているようです。これは、線形カルマン フィルターのような最適な推定器を使用していないという事実によるものですか、それとも EKF を使用して偏りのない推定値を取得することは可能でしょうか? これまでに 2 つの異なる実装を使用しましたが、どちらも同じ問題に遭遇しました。
フィルター出力のプロットを次に示します。
- 緑: 加速度計のみから推定された角度
- 青: 統合ジャイロスコープ出力
- 赤：リニアKF出力
- シアン: EKF 出力、オフセットに注意してください
1 回の反復の matlab コードを次に示します。
function [ q, wb ] = EKF2( a,w,dt )
persistent x P;
% Tuning paramaters
Q = [0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0.01, 0, 0;
0, 0, 0, 0, 0, 0.01, 0;
0, 0, 0, 0, 0, 0, 0.01];
R = [1000000, 0, 0;
0, 1000000, 0;
0, 0, 1000000;];
if isempty(P)
P = eye(length(Q))*10000; %Large uncertainty of initial values
x = [1, 0, 0, 0, 0, 0, 0]';
end
q0 = x(1);
q1 = x(2);
q2 = x(3);
q3 = x(4);
wxb = x(5);
wyb = x(6);
wzb = x(7);
wx = w(1);
wy = w(2);
wz = w(3);
z(1) = a(1);
z(2) = a(2);
z(3) = a(3);
z=z';
% Populate F jacobian
F = [ 1, (dt/2)*(wx-wxb), (dt/2)*(wy-wyb), (dt/2)*(wz-wzb), -(dt/2)*q1, -(dt/2)*q2, -(dt/2)*q3;
-(dt/2)*(wx-wxb), 1, (dt/2)*(wz-wzb), -(dt/2)*(wy-wyb), (dt/2)*q0, (dt/2)*q3, -(dt/2)*q2;
-(dt/2)*(wy-wyb), -(dt/2)*(wz-wzb), 1, (dt/2)*(wx-wxb), -(dt/2)*q3, (dt/2)*q0, (dt/2)*q1;
-(dt/2)*(wz-wzb), (dt/2)*(wy-wyb), -(dt/2)*(wx-wxb), 1, (dt/2)*q2, -(dt/2)*q1, (dt/2)*q0;
0, 0, 0, 0, 1, 0, 0;
0, 0, 0, 0, 0, 1, 0;
0, 0, 0, 0, 0, 0, 1;];
%%%%%%%%% PREDICT %%%%%%%%%
%Predicted state estimate
% x = f(x,u)
x = [q0 - (dt/2) * (-q1*(wx-wxb) - q2*(wy-wyb) - q3*(wz-wzb));
q1 - (dt/2) * ( q0*(wx-wxb) + q3*(wy-wyb) - q2*(wx-wzb));
q2 - (dt/2) * (-q3*(wx-wxb) + q0*(wy-wyb) + q1*(wz-wzb));
q3 - (dt/2) * ( q2*(wx-wxb) - q1*(wy-wyb) + q0*(wz-wzb));
wxb;
wyb;
wzb;];
% Re-normalize Quaternion
qnorm = sqrt(x(1)^2 + x(2)^2 + x(3)^2 + x(4)^2);
x(1) = x(1)/qnorm;
x(2) = x(2)/qnorm;
x(3) = x(3)/qnorm;
x(4) = x(4)/qnorm;
q0 = x(1);
q1 = x(2);
q2 = x(3);
q3 = x(4);
% Predicted covariance estimate
P = F*P*F' + Q;
%%%%%%%%%% UPDATE %%%%%%%%%%%
% Normalize Acc and Mag measurements
acc_norm = sqrt(z(1)^2 + z(2)^2 + z(3)^2);
z(1) = z(1)/acc_norm;
z(2) = z(2)/acc_norm;
z(3) = z(3)/acc_norm;
h = [-2*(q1*q3 - q0*q2);
-2*(q2*q3 + q0*q1);
-q0^2 + q1^2 + q2^2 - q3^2;];
%Measurement residual
% y = z - h(x), where h(x) is the matrix that maps the state onto the measurement
y = z - h;
% The H matrix maps the measurement to the states
H = [ 2*q2, -2*q3, 2*q0, -2*q1, 0, 0, 0;
-2*q1, -2*q0, -2*q3, -2*q2, 0, 0, 0;
-2*q0, 2*q1, 2*q2, -2*q3, 0, 0, 0;];
% Measurement covariance update
S = H*P*H' + R;
% Calculate Kalman gain
K = P*H'/S;
% Corrected model prediction
x = x + K*y; % Output state vector
% Re-normalize Quaternion
qnorm = sqrt(x(1)^2 + x(2)^2 + x(3)^2 + x(4)^2);
x(1) = x(1)/qnorm;
x(2) = x(2)/qnorm;
x(3) = x(3)/qnorm;
x(4) = x(4)/qnorm;
% Update state covariance with new knowledge
I = eye(length(P));
P = (I - K*H)*P; % Output state covariance
q = [x(1), x(2), x(3), x(4)];
wb = [x(5), x(6), x(7)];
end