卡尔曼滤波器实现 - 可能有啥问题
Posted
技术标签:
【中文标题】卡尔曼滤波器实现 - 可能有啥问题【英文标题】:Kalman Filter implementation - what could be wrong卡尔曼滤波器实现 - 可能有什么问题 【发布时间】:2012-08-07 15:31:35 【问题描述】:我很抱歉这么乏味,但我在十几篇文章的帮助下检查了我的代码几次,但我的 KF 仍然无法正常工作。 “不起作用”是指 KF 的估计是错误的。这是一个不错的 paste 真实、噪声和 KF 估计位置(只是一小块)。
我的示例与我找到的每个教程中的示例相同 - 我有一个位置和速度的状态向量。位置以米为单位,表示空气中的垂直位置。我的真实案例是跳伞(带降落伞)。在我的示例生成数据中,我假设我们从 3000m 开始,速度为 10m/s。
P.S.:我很确定矩阵计算没问题 - 逻辑一定有错误。
这里我生成数据:
void generateData(float** inData, float** noisedData, int x, int y)
inData[0][0]= 3000; //start position
inData[1][0]= -10; // 10m/s velocity; minus because we assume it's falling
noisedData[0][0]= 2998;
noisedData[1][0]= -10;
for(int i=1; i<x; i++)
inData[0][i]= inData[0][i-1] + inData[1][i-1];
inData[1][i]= inData[1][i-1]; //the velocity doesn't change for simplicity's sake
noisedData[0][i]=inData[0][i]+(rand()%6-3); //we add noise to real measurement
noisedData[1][i]=inData[1][i]; //velocity has no noise
这是我的实现(矩阵初始化基于Wikipedia Kalman example):
int main(int argc, char** argv)
srand(time(NULL));
float** inData = createMatrix(100,2); //2 rows, 100 columns
float** noisedData = createMatrix(100,2);
float** estData = createMatrix(100,2);
generateData(inData, noisedData, 100, 2);
float sampleRate=0.1; //10hz
float** A=createMatrix(2,2);
A[0][0]=1;
A[0][1]=sampleRate;
A[1][0]=0;
A[1][1]=1;
float** B=createMatrix(1,2);
B[0][0]=pow(sampleRate,2)/2;
B[1][0]=sampleRate;
float** C=createMatrix(2,1);
C[0][0]=1; //we measure only position
C[0][1]=0;
float u=1.0; //acceleration magnitude
float accel_noise=0.2; //acceleration noise
float measure_noise=1.5; //1.5 m standard deviation
float R=pow(measure_noise,2); //measure covariance
float** Q=createMatrix(2,2); //process covariance
Q[0][0]=pow(accel_noise,2)*(pow(sampleRate,4)/4);
Q[0][1]=pow(accel_noise,2)*(pow(sampleRate,3)/2);
Q[1][0]=pow(accel_noise,2)*(pow(sampleRate,3)/2);
Q[1][1]=pow(accel_noise,2)*pow(sampleRate,2);
float** P=createMatrix(2,2); //covariance update
P[0][0]=0;
P[0][1]=0;
P[1][0]=0;
P[1][1]=0;
float** P_est=createMatrix(2,2);
P_est[0][0]=P[0][0];
P_est[0][1]=P[0][1];
P_est[1][0]=P[1][0];
P_est[1][1]=P[1][1];
float** K=createMatrix(1,2); //Kalman gain
float** X_est=createMatrix(1,2); //our estimated state
X_est[0][0]=3000; X_est[1][0]=10;
// !! KALMAN ALGORITHM START !! //
for(int i=0; i<100; i++)
float** temp;
float** temp2;
float** temp3;
float** C_trans=matrixTranspose(C,2,1);
temp=matrixMultiply(P_est,C_trans,2,2,1,2); //2x1
temp2=matrixMultiply(C,P_est,2,1,2,2); //1x2
temp3=matrixMultiply(temp2,C_trans,2,1,1,2); //1x1
temp3[0][0]+=R;
K[0][0]=temp[0][0]/temp3[0][0]; // 1. KALMAN GAIN
K[1][0]=temp[1][0]/temp3[0][0];
temp=matrixMultiply(C,X_est,2,1,1,2);
float diff=noisedData[0][i]-temp[0][0]; //diff between meas and est
X_est[0][0]=X_est[0][0]+(K[0][0]*diff); // 2. ESTIMATION CORRECTION
X_est[1][0]=X_est[1][0]+(K[1][0]*diff);
temp=createMatrix(2,2);
temp[0][0]=1; temp[0][1]=0; temp[1][0]=0; temp[1][1]=1;
temp2=matrixMultiply(K,C,1,2,2,1);
temp3=matrixSub(temp,temp2,2,2,2,2);
P=matrixMultiply(temp3,P_est,2,2,2,2); // 3. COVARIANCE UPDATE
temp=matrixMultiply(A,X_est,2,2,1,2);
X_est[0][0]=temp[0][0]+B[0][0]*u;
X_est[1][0]=temp[1][0]+B[1][0]*u; // 4. PREDICT NEXT STATE
temp=matrixMultiply(A,P,2,2,2,2);
float** A_inv=getInverse(A,2);
temp2=matrixMultiply(temp,A_inv,2,2,2,2);
P_est=matrixAdd(temp2,Q,2,2,2,2); // 5. PREDICT NEXT COVARIANCE
estData[0][i]=X_est[0][0]; //just saving here for later to write out
estData[1][i]=X_est[1][0];
for(int i=0; i<100; i++) printf("%4.2f : %4.2f : %4.2f \n", inData[0][i], noisedData[0][i], estData[0][i]); // just writing out
return (EXIT_SUCCESS);
【问题讨论】:
我不会尝试回答这个问题,但假设-m/s
是否正确。我知道你在物体下落时得到了什么,但是连续的方程式是否意识到了这一点?保持正数并在需要的地方减去它会更好吗?好奇!
我试过了——还是一样! :)
【参考方案1】:
看起来您正在为该问题假设一个刚体模型。如果是这种情况,那么对于您正在解决的问题,当您进行流程更新以预测下一个状态时,我不会输入 u。也许我遗漏了一些东西,但输入 u 在生成数据中没有任何作用。
让我换一种说法,将 u 设置为 +1 看起来你的模型假设身体应该在 +x 方向上移动,因为在那个方向上有一个输入,但是测量告诉它去另一个方向大大地。因此,如果您对测量值施加很大的权重,它将向 -ve 方向移动,但如果您对模型施加很大的权重,则它应该向 +ve 方向移动。无论如何,根据生成的数据,我看不出将 u 设置为零的原因。
另一件事,你的采样率为 0.1 Hz,但是当你生成数据时,你假设它是一秒,因为每次采样,位置都会改变 -10 米每秒。
这是一个 matlab/octave 实现。
l = 1000;
Ts = 0.1;
y = 3000; %measurement to be fed to KF
v = -10; % METERS PER SECOND
t = [y(1);v]; % truth for checking if its working
for i=2:l
y(i) = y(i-1) + (v)*Ts;
t(:,i) = [y(i);v]; % copy to truth vector
y(i) = y(i) + randn; % noise it up
end
%%%%% Let the filtering begin!
% Define dynamics
A = [1, Ts; 0, 1];
B = [0;0];
C = [1,0];
% Steady State Kalman Gain computed for R = 0.1, Q = [0,0;0,0.1]
K = [0.44166;0.79889];
x_est_post = [3000;0];
for i=2:l
x_est_pre = A*x_est_post(:,i-1); % Process update! That is our estimate in case no measurement comes in.
%%% OMG A MEASUREMENT!
x_est_post(:,i) = x_est_pre + K*(-x_est_pre(1)+y(i));
end
【讨论】:
【参考方案2】:你做了很多奇怪的数组索引。
float** A=createMatrix(2,2);
A[0][0]=1;
A[0][3]=sampleRate;
A[1][0]=0;
A[1][4]=1;
在数组边界之外进行索引的预期结果是什么?
【讨论】:
我不知道这些数字是从哪里来的。在我的代码中,一切都很好。我这里也修好了。以上是关于卡尔曼滤波器实现 - 可能有啥问题的主要内容,如果未能解决你的问题,请参考以下文章
如果卡尔曼滤波器无法计算出稳定的卡尔曼增益,可能的原因是啥?