本帖最后由 yangbo18416 于 2020-5-11 17:58 编辑
目前我使用的卡尔曼滤波用于处理GPS定位情况，由于本人不太了解卡尔曼滤波的过程，因此很多资料是从网上查找到的。
目前系统的观测量位x,y vx vy,测量量Z为gps获取到的经纬度转成高斯坐标下的x y,状态转移矩阵A=[1,0,dt,0, 0,1,0,dt, 0,0,1,0, 0,0,0,1],测量矩阵H = [1,0,0,0 0,1,0,0];R噪声协方差位GPS提供的经纬度的方差
目前存在的情况如下：
在获取搞的高斯坐标下，设定一个人为原点后,比如，当前换算后的坐标为A:（3127409.64669,-19695075.21917)，然后系统原点设定为B:（3127411.929756，-19695090.395567），则进**尔曼滤波时的测量量传入的值A-B =(−2.283066,15.176397)，经过这样的处理后，得到的滤波数值感觉还行，Vx Vy观测量也还行；但是如果不减去原点，直接将获得的数据作为Z值传入计算，则得到的卡尔曼滤波结果相差很大。目前是百思不得其解！
相关程序如下：
MatrixXd X=MatrixXd(4, 1);
MatrixXd A=MatrixXd(4, 4);
MatrixXd H=MatrixXd(2, 4);
MatrixXd Q=MatrixXd(4, 4);
MatrixXd R=MatrixXd(2, 2);
MatrixXd P=MatrixXd(4, 4);
MatrixXd K=MatrixXd(4, 2);
MatrixXd Z=MatrixXd(2, 1);
A.setZero();
A(0,0) = 1;
A(0,2) = 1;
A(1,1) = 1;
A(1,3) = 1;
A(2,2) = 1;
A(3,3) = 1;
H.setZero();
H(0.0) = 1;
H(1,1) = 1;
// H<< 1,0,0,0,
// 0,1,0,0;
Q.setZero();
Q(0,0) = (double)0.5; //系统噪声只存在位移上，速度方向没有---速度通过推测而来
Q(1,1) = (double)0.5;
// Q(2,2) = (double)0.2;
// Q(3,3) = (double)0.2;
// Q<<(double)0.2, 0, 0, 0,
// 0, (double)0.2, 0, 0,
// 0, 0,(double)0.2, 0,
// 0, 0, 0, (double)0.2;
R.setZero();
R(0,0) = (double)0.4;
R(1,1) = (double)0.4;
// R<<(double)0.02,0,0,(double)0.02;
// R.setZero();
P.setZero();
P(0,0) = 0.8;
P(0,2) = 0.8;
P(1,1) = 0.8;
P(1,3) = 0.8;
P(2,2) = 0.8;
P(3,3) = 0.8;
// P<<1, 0, 0, 0,
// 0, 1, 0, 0,
// 0, 0, 1, 0,
// 0, 0, 0, 1;
K.setZero();
// K<< 0,0,0,0,0,0,0,0;
for(int i=0; i< MaxNum; i++)
{
X = kalfilter->kalman_input_exe(X,Z,0,Dt,R);
// std::cout <<"OrgX: "<< x_buf[i] << " OrgY: " << y_buf[i] <<" O_Dx: "<< (x_buf[i] - x_buf[i-1])/Dt << " O_Dy: " << (y_buf[i] - y_buf[i-1])/Dt << " OtX: " << X(0,0) << " OtY: " << X(1,0) << " Dx: " << X(2,0) << " Dy: " << X(3,0) <<std::endl;
// outfile << x_buf[i] << " " << y_buf[i] <<" "<< (x_buf[i] - x_buf[i-1])/Dt << " " << (y_buf[i] - y_buf[i-1])/Dt << " " << X(0,0) << " " << X(1,0) << " " << X(2,0)<< " " << X(3,0) <<std::endl;//输出到txt文档，用于matlab绘图
std::cout <<"OrgX: "<< x_buf[i] << " OrgY: " << y_buf[i] << " OtX: " << X(0,0) << " OtY: " << X(1,0) << " Dx: " << X(2,0) << " Dy: " << X(3,0) <<std::endl;
outfile <<setw(10)<<setiosflags(ios::fixed)<<setprecision(2)<<x_buf[i] << " " <<setw(10)<<setiosflags(ios::fixed)<<setprecision(2)<<y_buf[i] << " " <<setw(10)<<setiosflags(ios::fixed)<<setprecision(2)<< X(0,0) << " " <<setw(10)<<setiosflags(ios::fixed)<<setprecision(2)<< X(1,0) << " " << X(2,0)<< " " << X(3,0);
outfile << std::endl;//输出到txt文档，用于matlab绘图
Z << x_buf[i],y_buf[i];
R.setZero();
R(0,0) =pow(lat_stddev[i],2);
R(1,1) =pow(lon_stddev[i],2);
}
MatrixXd kalman_input_exe(MatrixXd stata_X,MatrixXd z,double u, double time,MatrixXd R)
{
m_delta_time=time;
m_R = R;
// A<< 1,0,(double)delta_time,0,
// 0,1,0,(double)delta_time,
// 0,0,1,0,
// 0,0,0,1;
// B<<0,0,0,0;
/* Predict */
// X=A*X+B*u;
m_X=m_A*stata_X; //无控制量--预测 先验估算值
//预测状态与真实状态的协方差矩阵，Pk'
m_P=m_A*m_P*m_A.transpose() + m_Q; //先验协方差
// /*update*/
//计算增益
m_K = m_P*m_H.transpose()*(m_H*m_P*m_H.transpose()+m_R).inverse();
m_X = m_X+m_K*(z-m_H*m_X); //后验估算值
m_P = m_P - m_K*m_H*m_P; //后验协方差
return m_X;
}
https://blog.csdn.net/qq_27251141/article/details/95047938
结贴：原因是在第一次数据处理时忘记将 最优观测量设置为 测量量，导致测量数据很大时需要很长的时间才能稳定！！！！
|
楼主
标题置顶
标题高亮
