卡尔曼滤波算法及C语言实现_源代码

卡尔曼滤波理论很容易就可以在MATLAB软件环境下实现,但是,实际的硬件板子上还是需要C语言,当然可以自动代码生成,还有一种就是直接手动编写C语言。

1.前言
在google上搜索卡尔曼滤波,很容易找到以下这个帖子:http://blog.csdn.net/lanbing510/article/details/8828109
帖子最后用matlab实现了kalman,然后博主的前面一些帖子也有详细转载相关贴子,自己也给出了一些源代码,例如转载的这篇卡尔曼滤波器通俗介绍:https://blog.csdn.net/weixin_38451800/article/details/85462129

2.卡尔曼滤波的C语言
  网上很多的是关于多维kalman实现。参照网上的一些代码,本文实现了一个一维地滤波,对于有C语言基础的同学来讲,理解起来应该很容易了。
百度百科里面有这个帖子:http://wenku.baidu.com/view/8523cb6eaf1ffc4ffe47ac24.html
  讲解的是一维kalman滤波器,但是最后printf出来的都是分立的值,看不出来什么。参考那段代码,改写成了下面这段代码,在labwindows里面绘制了一段曲线,效果就很直观了:

/*-------------------------------------------------------------------------------------------------------------*/ 
void KalmanFilter(unsigned int ResrcDataCnt,const double *ResrcData,double *FilterOutput,double ProcessNiose_Q,double MeasureNoise_R,double InitialPrediction)
{
        unsigned int i;
        double R = MeasureNoise_R;
        double Q = ProcessNiose_Q;
        double x_last = *ResrcData;
        double x_mid = x_last;
        double x_now;
        double p_last = InitialPrediction;
        double p_mid ;
        double p_now;
        double kg;

        for(i=0;i<ResrcDataCnt;i++)
        {
                x_mid=x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)
                p_mid=p_last+Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声
                kg=p_mid/(p_mid+R); //kg为kalman filter,R为噪声
                x_now=x_mid+kg*(*(ResrcData+i)-x_mid);//估计出的最优值
                p_now=(1-kg)*p_mid;//最优值对应的covariance
                
                *(FilterOutput + i)  = x_now;

                p_last = p_now; //更新covariance值
                x_last = x_now; //更新系统状态值
        }

}
/*-------------------------------------------------------------------------------------------------------------*/

参考上面的代码,优化了一下之后(运行在STM32上):

/*-------------------------------------------------------------------------------------------------------------*/
/*        
        Q:过程噪声,Q增大,动态响应变快,收敛稳定性变坏
        R:测量噪声,R增大,动态响应变慢,收敛稳定性变好        
*/

double KalmanFilter(const double ResrcData,
                                        double ProcessNiose_Q,double MeasureNoise_R,double InitialPrediction)
{
        double R = MeasureNoise_R;
        double Q = ProcessNiose_Q;

        static        double x_last;

        double x_mid = x_last;
        double x_now;

        static        double p_last;

        double p_mid ;
        double p_now;
        double kg;        

        x_mid=x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)
        p_mid=p_last+Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声
        kg=p_mid/(p_mid+R); //kg为kalman filter,R为噪声
        x_now=x_mid+kg*(ResrcData-x_mid);//估计出的最优值
                
        p_now=(1-kg)*p_mid;//最优值对应的covariance        

        p_last = p_now; //更新covariance值
        x_last = x_now; //更新系统状态值

        return x_now;                
}

/*-------------------------------------------------------------------------------------------------------------*/

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
3.接下来,是另外一个版本的单维卡尔曼滤波的C语言源代码:

#include "stdio.h"
#include "stdlib.h"
#include "math.h"
double frand()
{ return 2*((rand()/(double)RAND_MAX) - 0.5); // 随机噪声}

void main()
{ float x_last=0;
float p_last=0.02;
float Q=0.018;
float R=0.542;
float kg;
float x_mid;
float x_now;
float p_mid;
float p_now;
float z_real=0.56;//0.56
float z_measure;
float sumerror_kalman=0;
float sumerror_measure=0;
int i;
x_last=z_real+frand()*0.03;
x_mid=x_last;
for(i=0;i<20;i++)
{ x_mid=x_last;                                     //x_last=x(k-1|k-1),x_mid=x(k|k-1)
p_mid=p_last+Q;                                     //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q= 噪声
kg=p_mid/(p_mid+R);                                 //kg 为kalman filter ,R为噪声
z_measure=z_real+frand()*0.03;                      // 测量值
x_now=x_mid+kg*(z_measure-x_mid);                   // 估计出的最优值
p_now=(1-kg)*p_mid;                                 // 最优值对应的covariance
printf("Real position: %6.3f \n",z_real);           // 显示真值
printf("Mesaured position: %6.3f [diff:%.3f]\n",z_measure,fabs(z_real-z_measure));
                                                    // 显示测量值以及真值与测量值之间的误差
printf("Kalman position: %6.3f [diff:%.3f]\n",x_now,fabs(z_real - x_now));  // 显示kalman 估计值以及真值和卡尔曼估计值的误差
sumerror_kalman += fabs(z_real - x_now);            //kalman 估计值的累积误差
sumerror_measure += fabs(z_real-z_measure);         // 真值与测量值的累积误差
p_last = p_now;                                     // 更新covariance 值
x_last = x_now;                                     // 更新系统状态值
}
printf(" 总体测量误差 : %f\n",sumerror_measure);     // 输出测量累积误差
printf(" 总体卡尔曼滤波误差: %f\n",sumerror_kalman);  // 输出kalman 累积误差
printf(" 卡尔曼误差所占比例: %d%% \n",100-(int)((sumerror_kalman/sumerror_measure)*100));

——————————————————————————————————————————
部分内容转载自:https://www.amobbs.com/thread-5571611-1-1.html 和https://wenku.baidu.com/view/8523cb6eaf1ffc4ffe47ac24.html

### 回答1: 卡尔曼滤波算法是一种用于组合估计的算法,其目的是通过将传感器测量值与系统预测值进行加权平均,得到更准确的状态估计值。 卡尔曼滤波算法通过两个主要步骤实现:预测和更新。在预测步骤中,通过使用系统的动力学模型和先前状态的估计值,计算下一时刻的状态预测值和协方差矩阵。在更新步骤中,通过将传感器测量值与预测值进行对比,计算优化的状态估计值和协方差矩阵。 卡尔曼滤波算法的核心是协方差矩阵的更新,通过这个矩阵可以反映系统的状态估计的可信度,进而在预测和更新中进行权衡。卡尔曼滤波算法对噪声的建模也非常重要,通过对预测和更新中的噪声进行合理的建模,可以更准确地估计系统的状态。 在C语言实现卡尔曼滤波算法,需要使用矩阵运算和协方差矩阵的计算。可以通过定义结构体或数组来表示矩阵和向量,并使用相应的运算函数进行矩阵运算。同时,需要根据系统的动力学模型和传感器的特性,进行相应的参数配置和计算。 在实现中,首先需要初始化协方差矩阵和状态估计值。然后,在每个时间步骤中,根据系统的动力学模型进行状态预测,并计算预测的协方差矩阵。接着,根据传感器的测量值,计算卡尔曼增益,用于将预测值和测量值进行加权平均得到最终的状态估计值。最后,更新协方差矩阵。 实现卡尔曼滤波算法需要一定的数学知识和编程技巧。同时,需要根据具体的应用场景和系统特性进行参数的调整和优化,以获得更好的滤波效果。 ### 回答2: 卡尔曼滤波算法是一种用于估计线性系统状态的优化算法。它通过融合传感器的测量数据和系统模型,提供对系统状态的最优估计。卡尔曼滤波算法的核心思想是在每个时间步更新状态的估计,并同时计算估计的不确定性。 卡尔曼滤波算法的过程如下: 1. 初始化:给定系统的初始状态和初始不确定性。 2. 预测:根据系统模型,使用先前的状态估计和控制输入来预测当前状态的估计和不确定性。 3. 更新:通过融合传感器的测量数据和预测得到的状态估计,计算更新后的状态估计和不确定性。 在C语言实现卡尔曼滤波算法,需要定义系统模型、传感器模型和卡尔曼滤波算法的具体计算过程。首先,需要定义状态向量、状态转移矩阵、控制输入矩阵、观测向量和观测矩阵等系统模型参数。然后,可以利用这些参数实现卡尔曼滤波算法的预测和更新过程。预测过程中,根据系统模型和先前的状态估计,计算当前状态的预测估计和预测不确定性。更新过程中,利用传感器的测量数据和预测得到的状态估计,计算最优的状态估计和不确定性。最后,通过迭代进行多次预测和更新,即可得到系统状态的最优估计。 总结来说,卡尔曼滤波算法是一种用于估计线性系统状态的优化算法,通过融合传感器的测量数据和系统模型,提供对系统状态的最优估计。在C语言实现卡尔曼滤波算法,需要定义系统模型和实现卡尔曼滤波算法的预测和更新过程。
评论 6
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值