单片机论坛

 找回密码
 立即注册

QQ登录

只需一步,快速开始

搜索
查看: 54|回复: 0
打印 上一主题 下一主题
收起左侧

卡尔曼滤波算法程序,输入随机数,Q和R自己选

[复制链接]
跳转到指定楼层
楼主
suisex 发表于 2019-1-7 17:00 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
卡尔曼滤波算法,自己,输入为随机数,Q和R要自己选
主函数:

#include "kalman.h"

#include "stdio.h"

#include "stdlib.h"

void main(void)
{


        KalmanCountData k;
        //定义一个卡尔曼运算结构体
        Kalman_Filter_Init(&k);
        //讲运算变量初始化
        int n;        
        int a;
        srand((unsigned)time(NULL)); // 初始化随机种子,使rand()产生的随机数每次不一样
           for(a = 0;a<1000;a++)
        //测试1000次
        {
                //m,n为1到100的随机数
               
                n = 1+ rand() %100;
                //卡尔曼滤波,传递2个测量值以及运算结构体
      
        Kalman_Filter(&k,n);

                //打印结果
                printf("第%d次迭代\n  输入:%d, 输出滤波值为:%f\r\n",a+1,n,k.Filter_Value);
      
        }
}
kalman.h:
typedef struct                    //定义结构体
{
        float                                Filter_Value;                 //K-1时刻的系滤波值
        float                                A;                           //系统参数,一维模型中通常选为1
        float                                H;                          //系统测量参数,一维模型选择为1
        float                                Kg;                        //卡尔曼增益
        float                                Q;                           //过程噪声偏差
        float                                 R;                          //测量噪声偏差
        float                                 P;                      //估计误差协方差     
} KalmanCountData;

void Kalman_Filter_Init(KalmanCountData * Kalman_Struct)             //卡尔曼滤波器初始化
{
    Kalman_Struct->A = 1;
    Kalman_Struct->H = 1;
    Kalman_Struct->P = 3;                //后验状态估计值方差的初始值
    Kalman_Struct->Q = 0.1;               //过程噪声偏差,实验测的 ,要填的!
    Kalman_Struct->R = 10;                 //测量噪声偏差,要填的!
    Kalman_Struct->Filter_Value = 0;                   //初始化设置k-1时刻的系统滤波后的值为0
}
double Kalman_Filter(KalmanCountData* Kalman_Struct, double Last_Measurement){
    double PredictValue = Kalman_Struct->Filter_Value*Kalman_Struct->A;     //先验估计的预测值
    Kalman_Struct->P = Kalman_Struct->A*Kalman_Struct->P*Kalman_Struct->A+Kalman_Struct->Q; //求协方差
    double prevalve = Kalman_Struct->Filter_Value;  //记录上一次(k-1)的值
    Kalman_Struct->Kg = Kalman_Struct->P*Kalman_Struct->H/(Kalman_Struct->H*Kalman_Struct->P*Kalman_Struct->H + Kalman_Struct->R);
    /*
        卡尔曼增益Kg = P(k|k-1) H’ / (H P(k|k-1) H’ + R)
    */
    Kalman_Struct->Filter_Value = PredictValue + (Last_Measurement-PredictValue) * Kalman_Struct->Kg;
    /*
        修正结果,更新预测值;X(k|k) =  X(k|k-1) + (Z(k) - H* X(k|k-1)) * Kg
    */
    Kalman_Struct->P = (1 - Kalman_Struct->Kg * Kalman_Struct->H) * Kalman_Struct->P;
    //更新后验估计的协方差,P(k|k) = (1 - Kg * H) * P(k|k-1)
    return Kalman_Struct->Filter_Value;
}


Kalman.zip

89.46 KB, 下载次数: 3, 下载积分: 黑币 -5

评分

参与人数 1黑币 +50 收起 理由
admin + 50 共享资料的黑币奖励!

查看全部评分

分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏 转播转播 分享分享 分享淘帖 顶 踩
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

QQ|手机版|小黑屋|单片机论坛 |51黑电子论坛单片机.

Powered by 单片机教程网

快速回复 返回顶部 返回列表