找回密码
 立即注册

QQ登录

只需一步,快速开始

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

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

[复制链接]
跳转到指定楼层
楼主
ID:463943 发表于 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, 下载次数: 38, 下载积分: 黑币 -5

评分

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

查看全部评分

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

使用道具 举报

沙发
ID:640288 发表于 2019-11-12 11:10 | 只看该作者
您好,这个程序中您说的过程测量噪声是实验测的,能给点测量方法吗,最近在做卡尔曼滤波算法对系统误差测量一筹莫展。
回复

使用道具 举报

板凳
ID:640288 发表于 2019-11-12 11:15 | 只看该作者
您好,我想请问一下系统测量误差实验测得的可以分享一下实验测量的步骤吗?
回复

使用道具 举报

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

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

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