关于卡尔曼滤波的疑问和讨论

R88 · 发表于 2014-7-22 11:11:48

这里方便初学只说一维的（感觉多维的其实就把多维的写在一个数组里面--类似矩阵，程序看起来更简洁了，计算的过程中还是各算各得）。看了温度的那个例子，感觉很形象，照着一个程序看了一下，感觉应用上还算明白了一点，但是原理上还得看kalman的论文（一堆计算，一堆符号，太吓人了）。下面是一位网友照比温度的那个例子写出来的程序，不知道他的测试结果怎么样？

但是实际上那个温度的例子，文档中后面给出了建模的大致过程：

可以看到上面的建模和实际程序不太一样，Kg的计算图一中用的是标准差，而建模中用的方差。那温度的那个例子中作者岂不前后冲突了？还是说第二个图是第一个图的精简版，速度其较快，精度比其差？论坛中大部分的程序也都是用的这个建模例子写的，下面是其中一个：

还有一点温度例子中，最后的matlab仿真文件很多人说是错误的，我仿真了一下，波形也不对，传一个网友传的：

clc
clear
N=200;
w=randn(1,N); %标准正态分布的随机数
for t=1:N
z(t)=25+w(t)*3; %测量值
end
mean(z)
q1=std(z); %求标准差
R=q1^2; %观测噪声
V=0.1*randn(1,N);
q2=std(V);
Q=q2^2; %模型噪声
p(1)=10;
x(1)=0;
for t=2:N;
x1(t)=x(t-1); %预测状态
p1(t)=p(t-1)+Q; %预测估计协方差
k(t)=p1(t)/(p1(t)+R); %最优卡尔曼增益
x(t)=x1(t)+k(t)*(z(t)-x1(t)); %更新状态估计
p(t)=(1-k(t))*p1(t); %更新协方差估计
end
t=1:N;
plot([0,N],[25,25],'g',t,z,'b',t,x,'r');
legend('真实值','测量值','估计值');

复制代码

仿真结果也是正确的：

还有一点，是关于零时刻的初始值和Q和R的值，大家都不知道如何选择，下面是部分高手给的回复：
初始值的估计：为了令卡尔曼滤波器开始工作，我们需要告诉卡尔曼两个零时刻的初始值，是X(0|0)和P(0|0)。他们的值不用太在意，随便给一个就可以了，因为随着卡尔曼的工作，X会逐渐的收敛（收敛到25度）。但是对于P，一般不要取0，因为这样可能会令卡尔曼完全相信你给定的X(0|0)是系统最优的，从而使算法不能收敛。我选了X(0|0)=1度，P(0|0)=10。（学院派的一种说法是：卡尔曼的收敛性与最初的系统估计关系不是很大，只要不为0，最终都会有比较理想的收敛效果。在最终的收敛完场之后，无论最初赋值为多少，都基本会稳定到某一个值）
QR的值：用通俗的语言描述下大概意思(A=1的情况下)：Q值大小只影响初期预估值的稳定跟踪时间，稳定跟踪后几乎不起作用，所以Q=0都可以。但R值一定是非零数值，R值越小滤波效果越差，但跟踪越快，反之，R值越大滤波越平滑，但跟踪越慢。所以要达到预期效果，只要调节R值即可。多说一句卡尔曼滤波做为最优线性观测器，严格来讲是没有“参数”的，最优的东西是不应该受参数而转移的，系统误差矩阵和测量误差矩阵应该叫“模型”，它们的确定属于模型辨识问题而不是参数整定问题。

就看了一天的资料，很多东西不一定对的。大家说说自己的看法吧。

babyhua · 发表于 2014-7-22 14:30:45

你这个是一维的，有多维的例子吗？

R88 · 发表于 2014-7-22 15:04:20

babyhua 发表于 2014-7-22 14:30
你这个是一维的，有多维的例子吗？

没有啊，用不到没研究。

1ongquan · 发表于 2014-8-14 00:07:45

mark

笑笑我笑了 · 发表于 2014-8-14 01:10:12

我觉得卡尔曼滤波器就三个部分，
一是利用系统模型估计下一时刻的系统状态，
二是利用下一时刻的系统输出来估计下一时刻的系统状态，
三是给予这两个估计值一定的权重然后平均得出系统状态的最优值。
没看过卡尔曼滤波器的具体推导，所以我也不知道为什么这么做有用，但是的确有用。
关于QR值的说法，我的观点和LZ是一样的。
卡尔曼滤波器还有其他变种，比如扩展卡尔曼滤波器。传统卡尔曼滤波器要求系统是线性系统，扩展卡尔曼滤波器就没这个要求了。
还有其他几种不记得了，最近在研究状态机，没时间看这些。

附带一个用来计算姿态角的卡尔曼滤波器，多维的。

%kalman 2014@L.
clear;
%define var
ACC_LSB = 16384/9.8;
GYR_LSB = 65.5/57.3;
len = 1000;
len2 = 300;
ERROR = 1:len2;
X_ACC = 1:len;
Y_ACC = 1:len;
Z_ACC = 1:len;
X_GYRO = 1:len;
Y_GYRO = 1:len;
Z_GYRO = 1:len;
theta_ANGLE = 1:len2;
fal_ANGLE = 1:len2;
pu_ANGLE = 1:len2;
A = ones(4);
X = [1, 0, 0, 0]';
P = [1 1 1 1];
P = diag(P);
Q = [2e-5, 2e-5, 2e-5, 2e-5];
Q = diag(Q);
R = [5e-1,5e-1,5e-1];
R = diag(R);
I = [1 1 1 1];
I = diag(I);
H = ones(3, 4);
h = ones(3, 3);
t = 0.001;
S = ones(3,1);
T1 = ones(3,1);
T2 = ones(3,1);
T3 = ones(3,1);
%read data file
textData = importdata('/home/life/Code/ARM/STM32/F103/MPU6050/test.dat');
%X_ACC
for i = 1:len
X_ACC(i) = textData(i,2)/ACC_LSB;
end
%Y_ACC
for i = 1:len
Y_ACC(i) = textData(i,3)/ACC_LSB;
end
%Z_ACC
for i = 1:len
Z_ACC(i) = textData(i,4)/ACC_LSB;
end
%X_GYRO
for i = 1:len
X_GYRO(i) = textData(i,5)/GYR_LSB;
end
%X_GYRO
for i = 1:len
Y_GYRO(i) = textData(i,6)/GYR_LSB;
end
%X_GYRO
for i = 1:len
Z_GYRO(i) = textData(i,7)/GYR_LSB;
end
%main loop
for i = 1:len2
%calculate A
% A(1,1) = 1;
% A(1,2) = exp(-t*X_GYRO(i));
% A(1,3) = exp(-t*Y_GYRO(i));
% A(1,4) = exp(-t*Z_GYRO(i));
%
% A(2,1) = exp(t*X_GYRO(i));
% A(2,2) = 1;
% A(2,3) = exp(t*Z_GYRO(i));
% A(2,4) = exp(t*Y_GYRO(i));
%
% A(3,1) = exp(t*Y_GYRO(i));
% A(3,2) = exp(-t*Z_GYRO(i));
% A(3,3) = 1;
% A(3,4) = exp(t*X_GYRO(i));
%
% A(4,1) = exp(t*Z_GYRO(i));
% A(4,4) = 1;
% A(4,2) = exp(t*Y_GYRO(i));
% A(4,3) = exp(-t*X_GYRO(i));
%calculate s
T1(1) = 0.5*(X_GYRO(3*i)+X_GYRO(3*i+1))*t;
T1(2) = 0.5*(Y_GYRO(3*i)+Y_GYRO(3*i+1))*t;
T1(3) = 0.5*(Y_GYRO(3*i)+Y_GYRO(3*i+1))*t;
T2(1) = 0.5*(X_GYRO(3*i+1)+X_GYRO(3*i+2))*t;
T2(2) = 0.5*(Y_GYRO(3*i+1)+Y_GYRO(3*i+2))*t;
T2(3) = 0.5*(Z_GYRO(3*i+1)+Z_GYRO(3*i+2))*t;
T3(1) = 0.5*(X_GYRO(3*i+2)+X_GYRO(3*i+3))*t;
T3(2) = 0.5*(Y_GYRO(3*i+2)+Y_GYRO(3*i+3))*t;
T3(3) = 0.5*(Z_GYRO(3*i+2)+Z_GYRO(3*i+3))*t;
S = T1+T2+T3+(33/80)*cross(T1, T2)+(57/80)*cross(T2, (T3-T1));
%
% q0 = cos(sqrt(S(1)^2 + S(2)^2 + S(3)^2));
% q1 = (S(1)/sqrt(S(1)^2 + S(2)^2 + S(3)^2))*sin(S(1)/sqrt(S(1)^2 + S(2)^2 + S(3)^2));
% q2 = (S(2)/sqrt(S(1)^2 + S(2)^2 + S(3)^2))*sin(S(2)/sqrt(S(1)^2 + S(2)^2 + S(3)^2));
% q3 = (S(3)/sqrt(S(1)^2 + S(2)^2 + S(3)^2))*sin(S(3)/sqrt(S(1)^2 + S(2)^2 + S(3)^2));
sx = S(1);
sy = S(2);
sz = S(3);
% sx = X_GYRO(i)*t;
% sy = Y_GYRO(i)*t;
% sz = Z_GYRO(i)*t;
s = 0.25*(sx^2+sy^2+sz^2);
ac = 1-0.5*s+s^2/24;
as = 0.5*(1 - s/6 + s^2/120);
% A(1,1) = q0;
% A(1,2) = -q1;
% A(1,3) = -q2;
% A(1,4) = -q3;
%
% A(2,1) = q1;
% A(2,2) = q0;
% A(2,3) = -q3;
% A(2,4) = q2;
%
% A(3,1) = q2;
% A(3,2) = q3;
% A(3,3) = q0;
% A(3,4) = -q1;
%
% A(4,1) = q3;
% A(4,2) = -q2;
% A(4,3) = q1;
% A(4,4) = q0;
A(1,1) = ac;
A(1,2) = -as*sx;
A(1,3) = -as*sy;
A(1,4) = -as*sz;
A(2,1) = as*sx;
A(2,2) = ac;
A(2,3) = -as*sz;
A(2,4) = as*sy;
A(3,1) = as*sy;
A(3,2) = as*sz;
A(3,3) = ac;
A(3,4) = -as*sx;
A(4,1) = as*sz;
A(4,2) = as*sy;
A(4,3) = -as*sx;
A(4,4) = ac;
%q = sqrt(t*X_GYRO(i)^2+t*Y_GYRO(i)^2+t*Z_GYRO(i)^2);
%R = [(1 - ((0.5*q)^2)/2), 0.5*X_GYRO(i), 0.5*Y_GYRO(i), 0.5*Z_GYRO(i)]';
%A = A*R;
%time updata
X = A*X;
P = A*P*A' + Q;
%measure updata
%[ ax*q0 + ay*q3 - az*q2, ax*q1 + ay*q2 + az*q3, ay*q1 - ax*q2 - az*q0, ay*q0 - ax*q3 + az*q1]
%[ ay*q0 - ax*q3 + az*q1, ax*q2 - ay*q1 + az*q0, ax*q1 + ay*q2 + az*q3, az*q2 - ay*q3 - ax*q0]
%[ ax*q2 - ay*q1 + az*q0, ax*q3 - ay*q0 - az*q1, ax*q0 + ay*q3 - az*q2, ax*q1 + ay*q2 + az*q3]
%[ 2*q3*vz - 2*q4*vy, 2*q3*vy + 2*q4*vz, 2*q2*vy - 4*q3*vx + 2*q1*vz, 2*q2*vy - 4*q3*vx + 2*q1*vz]
%[ 2*q4*vx - 2*q2*vz, 2*q3*vx - 4*q2*vy - 2*q1*vz, 2*q2*vx + 2*q4*vz, 2*q1*vx - 4*q4*vy + 2*q3*vz]
%[ 2*q2*vy - 2*q3*vx, 2*q4*vx + 2*q1*vy - 4*q2*vz, 2*q4*vy - 2*q1*vx - 4*q3*vz, 2*q2*vx + 2*q3*vy]
j = i;
%calculate H
H(1,1) = 2*X(3)*Z_ACC(j)-2*X(4)*Y_ACC(j);
H(1,2) = 2*X(3)*Y_ACC(j)+2*X(4)*Z_ACC(j);
H(1,3) = 2*X(2)*Y_ACC(j)-4*X(3)*X_ACC(j)+2*X(1)*Z_ACC(j);
H(1,4) = 2*X(2)*Y_ACC(j)-4*X(3)*X_ACC(j)+2*X(1)*Z_ACC(j);
H(2,1) = 2*X(4)*X_ACC(j)-2*X(2)*Z_ACC(j);
H(2,2) = 2*X(3)*X_ACC(j)-4*X(2)*Y_ACC(j)-2*X(1)*Z_ACC(j);
H(2,3) = 2*X(2)*X_ACC(j)+2*X(4)*Z_ACC(j);
H(2,4) = 2*X(1)*X_ACC(j)-4*X(4)*Y_ACC(j)+2*X(3)*Z_ACC(j);
H(3,1) = 2*X(2)*Y_ACC(j)-2*X(3)*X_ACC(j);
H(3,2) = 2*X(4)*X_ACC(j)+2*X(1)*Y_ACC(j)-4*X(2)*Z_ACC(j);
H(3,3) = 2*X(4)*Y_ACC(j)-2*X(1)*X_ACC(j)-4*X(3)*Z_ACC(j);
H(3,4) = 2*X(2)*X_ACC(j)+2*X(3)*Y_ACC(j);
% H(1,1) = 2*X(3)*Z_ACC(i)-2*X(4)*Y_ACC(i);
% H(1,2) = 2*X(3)*Y_ACC(i)+2*X(4)*Z_ACC(i);
% H(1,3) = 2*X(2)*Y_ACC(i)-4*X(3)*X_ACC(i)+2*X(1)*Z_ACC(i);
% H(1,4) = 2*X(2)*Y_ACC(i)-4*X(3)*X_ACC(i)+2*X(1)*Z_ACC(i);
%
% H(2,1) = 2*X(4)*X_ACC(i)-2*X(2)*Z_ACC(i);
% H(2,2) = 2*X(3)*X_ACC(i)-4*X(2)*Y_ACC(i)-2*X(1)*Z_ACC(i);
% H(2,3) = 2*X(2)*X_ACC(i)+2*X(4)*Z_ACC(i);
% H(2,4) = 2*X(1)*X_ACC(i)-4*X(4)*Y_ACC(i)+2*X(3)*Z_ACC(i);
%
% H(3,1) = 2*X(2)*Y_ACC(i)-2*X(3)*X_ACC(i);
% H(3,2) = 2*X(4)*X_ACC(i)+2*X(1)*Y_ACC(i)-4*X(2)*Z_ACC(i);
% H(3,3) = 2*X(4)*Y_ACC(i)-2*X(1)*X_ACC(i)-4*X(3)*Z_ACC(i);
% H(3,4) = 2*X(2)*X_ACC(i)+2*X(3)*Y_ACC(i);
% H(1,1) = X(1)*X_ACC(i)+X(4)*Y_ACC(i)-X(3)*Z_ACC(i);
% H(1,2) = X(2)*X_ACC(i)+X(3)*Y_ACC(i)+X(4)*Z_ACC(i);
% H(1,3) = X(2)*X_ACC(i)-X(3)*Y_ACC(i)-X(1)*Z_ACC(i);
% H(1,4) = X(1)*X_ACC(i)-X(4)*Y_ACC(i)+X(2)*Z_ACC(i);
%
% H(2,1) = X(1)*X_ACC(i)-X(4)*Y_ACC(i)+X(2)*Z_ACC(i);
% H(2,2) = X(3)*X_ACC(i)-X(2)*Y_ACC(i)+X(1)*Z_ACC(i);
% H(2,3) = X(2)*X_ACC(i)+X(3)*Y_ACC(i)+X(4)*Z_ACC(i);
% H(2,4) = X(3)*X_ACC(i)-X(4)*Y_ACC(i)-X(1)*Z_ACC(i);
%
% H(3,1) = X(3)*X_ACC(i)-X(2)*Y_ACC(i)+X(1)*Z_ACC(i);
% H(3,2) = X(4)*X_ACC(i)-X(1)*Y_ACC(i)-X(2)*Z_ACC(i);
% H(3,3) = X(1)*X_ACC(i)+X(4)*Y_ACC(i)-X(3)*Z_ACC(i);
% H(3,4) = X(2)*X_ACC(i)+X(3)*Y_ACC(i)+X(4)*Z_ACC(i);
%calculate h
%[ q0^2 + q1^2 - q2^2 - q3^2, 2*q0*q3 + 2*q1*q2, 2*q1*q3 - 2*q0*q2]
%[ 2*q1*q2 + 2*q0*q3, q0^2 - q1^2 + q2^2 - q3^2, 2*q0*q1 - 2*q2*q3]
%[ 2*q0*q2 - 2*q1*q3, 2*q2*q3 + 2*q0*q1, q0^2 - q1^2 - q2^2 + q3^2]
h(1,1) = X(1)^2 + X(2)^2 - X(3)^2 - X(4)^2;
h(1,2) = 2*(X(1)*X(4) + X(2)*X(3));
h(1,3) = 2*(X(2)*X(4) + X(1)*X(3));
h(2,1) = 2*(X(2)*X(3) - X(1)*X(4));
h(2,2) = X(1)^2 - X(2)^2 + X(3)^2 - X(4)^2;
h(2,3) = 2*(X(1)*X(2) + X(3)*X(4));
h(3,1) = 2*(X(1)*X(3) + X(2)*X(4));
h(3,2) = 2*(X(3)*X(4) - X(1)*X(2));
h(3,3) = X(1)^2 - X(2)^2 - X(3)^2 + X(4)^2;
%H = 2*H;
%calculate Kg
Kg = P*H'/(H*P*H' + R);
%calculate new X and P
Z = [X_ACC(i), Y_ACC(i), Z_ACC(i)];
Z = Z';
X = X + Kg*(Z - H*X);
P = (I - Kg*H)*P;
ERROR(i) = 1- sqrt(X(1)^2+X(2)^2+X(3)^2+X(4)^2);
X(1) = X(1)/sqrt(X(1)^2+X(2)^2+X(3)^2+X(4)^2);
X(2) = X(2)/sqrt(X(1)^2+X(2)^2+X(3)^2+X(4)^2);
X(3) = X(3)/sqrt(X(1)^2+X(2)^2+X(3)^2+X(4)^2);
X(4) = X(4)/sqrt(X(1)^2+X(2)^2+X(3)^2+X(4)^2);
theta_ANGLE(i) = asin(2*(X(2)*X(4)-X(1)*X(3)))*57.3;
fal_ANGLE(i) = atan2(2*X(1)*X(2)+2*X(3)*X(4), X(1)^2-X(2)^2-X(3)^2+X(4)^2)*57.3;
pu_ANGLE(i) = atan2(2*X(1)*X(4) +2*X(2)*X(3),X(1)^2+X(2)^2-X(3)^2-X(4)^2)*57.3;
end

复制代码

Edwardwei · 发表于 2014-8-14 09:23:43

我也试过，MPU6050输出数据，边输边滤波，得到的效果很不错！但是要如何在动态下滤波呢，就是测量的数据不是这种静态不变的，而是随时在变化的。我试过把MPU6050传感器移动，使加速度值是变化的，滤波的结果与真实值差很多，直到不动传感器，滤波结果在很长一段时间后才会收敛。

生来孤独 · 发表于 2014-8-19 20:49:38

mark

nhw1234 · 发表于 2014-8-19 23:09:27

学习了，感谢楼主

关于卡尔曼滤波的疑问和讨论

本帖子中包含更多资源

阿莫论坛20周年了！感谢大家的支持与爱护！！

浏览过的版块