如何在得到四元数之后进行卡尔曼滤波？-从未拥有的回答

如何在得到四元数之后进行卡尔曼滤波？

2019-07-14 22:32发布

13228 2

用卡尔曼滤波处理四元数，以前用过单变量的卡尔曼滤波，但是这一次需要处理四个变量，有些不太清楚了，有做过的坛友指教一下原理和公式~~下面是我四元数的后半部分程序：

tica, SimSun, sans-serif">

复制代码这是单变量的卡尔曼滤波器：

float KalmanGain;// 卡尔曼增益
float EstimateCovariance;//估计协方差
float MeasureCovariance;//测量协方差
float EstimateValue;//估计值
void KalmanFilterInit(void)
{
EstimateValue = 0;
EstimateCovariance = 1;
MeasureCovariance = 2;
}
float KalmanFilter(float Measure)
{
//计算卡尔曼增益
KalmanGain=EstimateCovariance*sqrt(1/(EstimateCovariance*EstimateCovariance+MeasureCovariance*MeasureCovariance));
//计算本次滤波估计值
EstimateValue=EstimateValue+KalmanGain * (Measure-EstimateValue);
//更新估计协方差
EstimateCovariance=sqrt(1-KalmanGain) * EstimateCovariance;
//更新测量方差
MeasureCovariance=sqrt(1-KalmanGain) * MeasureCovariance;
//返回估计值
return EstimateValue;
}

复制代码

友情提示: 此问题已得到解决，问题已经关闭,关闭后问题禁止继续编辑，回答。

2条回答

2019-07-15 06:31

人走了发表于 2015-5-22 16:29
我觉得要是单变量卡尔曼用起来的话，先把加速度做卡尔曼（我现在用的是20次滑动平均），在传入四元素里面融合，应该会有不少的提升。

不要把卡尔曼和普通滤波混淆了。你对加速度做的只是个平均滤波，那可算不上卡尔曼。