关于四元数卡尔曼的一些重大问题

jack_leon

本人最近在研究四元数卡尔曼滤波算法(加速度计，陀螺仪，磁力计). 目前有些疑问始终没有解决, 如下:1. 关于过程噪声和观测噪声的表达形式(目前我认为是v = [vx; vy; vz], vx = randn(1, n)这样的形式)如何取值?. 此外还有他们的协方差矩阵又是什么形式的?另外还将观测协方差矩阵R 化成R = p * I_3(3x3单位矩阵)(equation(33)). 但是格式(矩阵运算格式eg.(4x4) * (4x3) = (4x3))上又对不到.
2. 关于短时间内的角度增量有什么取值要求或技巧?
3. 关于传感器输出数据如何取值?
4. 程序（程序语法没错，根据论文用matlab写的）循环若干次后协方差矩阵经常显示为NaN.......论文上的是用指数分布
q(k +1) = exp(0.5 * OuMu[w] * T)q(k),T为采样周期
求哪位大神帮帮忙啊, 有做过的能不能提供各种数据啊，程序运行没有错误，主要是一些矩阵格式和原始数据的问题．
求帮忙！！！
:'(:'(:'(:'(
另附代码
% 四元数卡尔曼滤波
%-------------------------------------------------------------------------%
clear all
clc
%----------------------------init---------------------------%
n_iter = 100; % 采样个数
T = 0.001; % 采样时间
%%%%%%%%%%%%%%%%%%%%定义过程噪声 vQ %%%%%%%%%%%%%%%%%%%%%
vQo = zeros(1, 100);
vQx = randn(1, 100);
vQy = randn(1, 100);
vQz = randn(1, 100);
vQ1 = [vQx; vQy; vQz];
vQ2 = [vQo; vQx; vQy; vQz]; % 试验
Q2 = cell(1, 100);
Q = zeros(1, 100);
for i = 1 : 100
    Q2{i} = vQ2(:, i) * vQ2(:, i)';
end
for j = 1 : 100
    Q(j) = var(vQ1(:, j));　% equation(30)下面一段话推测的
end
%%%%%%%%%%%%%%%%%%%%定义观测噪声 vR %%%%%%%%%%%%%%%%%%%%%
vRo = zeros(1, 100);
vRx = randn(1, 100);
vRy = randn(1, 100);
vRz = randn(1, 100);
vR1 = [vRx; vRy; vRz];
vR2 = [vRo; vRx; vRy; vRz];
R2 = cell(1, 100);
R = zeros(1, 100);
for i = 1 : 100
    R2{i} = vR2(:, i) * vR2(:, i)';
end
for j = 1 : 100
    R(j) = var(vR1(:, j));
end
%%%%%%%%%%%%%%%%%%%%定义陀螺仪测量角加速度 angle %%%%%%%%%%%%%%%%%%%%%
angle_x = 1 * ones(1, 100);
angle_y = 2 * ones(1, 100);
angle_z = 3 * ones(1, 100);
angle = [angle_x; angle_y; angle_z];
%%%%%%%%%%%定义t时刻四元数和过程协方差的估计值（q_guji and P_qguji）%%%%%%%%%%
q_guji = cell(1, 100); % t时刻对qt的估计
q_guji{1} = [1 0 0 0]'; % 初始化估计值
P_qguji = cell(1, 100); % t时刻对过程噪声的协方差P_qt的估计
I_four = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];
P_qguji{1} = I_four; % 初始化估计值
%%%%%%%%%%%定义观测协方差的预测值和估计值 P_vyuce and P_vguji%%%%%%%%%%%
P_vyuce = cell(1, 100);
%%%%%%%%%%%%%定义四元数q_yuce表示为：t时刻对t+1时刻的q的预测%%%%%%%%%%%%%%%
% 从cell的第二项开始
q_yuce = cell(1, 100);
P_qyuce = cell(1, 100);
%%%%%%%%%%%%定义安装在载体坐标系上的传感器输出 b 和 参考坐标系　r %%%%%%%%%%%%%
bx = ones(1, 100);
by = ones(1, 100);
bz = ones(1, 100);
b = [bx; by; bz];
rx = 9.8 * ones(1, 100) + 0.02 * randn(1, 100);
ry = 3.2 * ones(1, 100) + 0.05 * randn(1, 100);
rz = 6.4 * ones(1, 100) + 0.05 * randn(1, 100);
r = [rx; ry; rz];
%%%%%%%%%%%%%%%%%%%%%定义1/2和差算子 S(和) 和 d(差) %%%%%%%%%%%%%%%%%%%%%%%%
s = cell(1, 100);
d = cell(1, 100);
%%%%%%%%%%%%%%%%%%%%定义预测矩阵 H 和 kalman gain%%%%%%%%%%%%%%%%%%%%%
H = cell(1, 100);
K= cell(1, 100);
%%%%%%%%%%%其他%%%%%%%%%%%%%%%%%%
B = cell(1, 100);
S = cell(1, 100);
C = cell(1, 100);
%-------------------------------------------------------------------------%
for t = 2 : n_iter
    %%%%%%%%%%%%%%%%%%状态方程传播%%%%%%%%%%%%%%%%%%%%%%
    A = [0, -angle_x(t - 1), -angle_y(t - 1), -angle_z(t - 1);
         angle_x(t - 1), 0, angle_z(t - 1),   -angle_y(t - 1);
         angle_y(t - 1), -angle_z(t - 1), 0,   angle_x(t - 1);
         angle_z(t - 1),  angle_y(t - 1), -angle_x(t - 1), 0] * T;
    C{t} = exp((0.5 * A));
    q_yuce{t} = C{t} * q_guji{t - 1};
    M = q_yuce{t} * q_yuce{t}' + P_qguji{t - 1};
    P_qyuce{t} = (C{t} * P_qguji{t - 1} * C{t}') + (1 / 4) * (trace(M) * I_four - M) * Q(t);
    %%%%%%%%%%%%%%%%观测方程更新%%%%%%%%%%%%%%%%%%%%%%%%
    s{t} = 0.5 * (b(:, t) + r(:, t));
    d{t} = 0.5 * (b(:, t) - r(:, t));
    H{t} = [0, -0.5*(b(1, t) - r(1, t)), -0.5*(b(2, t) - r(2, t)), -0.5*(b(3, t) - r(3, t));
           0.5*(b(1, t) - r(1, t)),    0, 0.5*(b(3, t) + r(3, t)), -0.5*(b(2, t) + r(2, t));
           0.5*(b(2, t) - r(2, t)), -0.5*(b(3, t) + r(3, t)),    0, 0.5*(b(1, t) + r(1, t));
           0.5*(b(3, t) - r(3, t)), 0.5*(b(2, t) + r(2, t)), -0.5*(b(1, t) + r(1, t)),   0];　% 观测矩阵
    B{t}= [0 -b(1, t), -b(2, t), -b(3, t);
           b(1, t),  0, b(3, t), -b(2, t);
           b(2, t), -b(3, t),  0, b(1, t);
           b(3, t), b(2, t), -b(1, t), 0];
    P_vyuce{t} = 0.25 * R2{t - 1} *(trace(M) * I_four - M - (B{t} * M * B{t}')); % R2有问题
    S{t} = H{t} * P_qyuce{t} * (H{t})' + R(t);
    K{t} = P_vyuce{t} * (H{t})' * S{t};
    q_guji{t} = (I_four - K{t} * H{t}) * q_yuce{t};
    q_guji{t} = q_guji{t} / norm(q_guji{t}, 2); % 归一化
    P_qguji{t} = (I_four - K{t} * H{t}) * P_qyuce{t} * (I_four - K{t} * H{t})';
end

求大神们解答