卡尔曼滤波C++代码实现.zip

用C++代码实现卡尔曼滤波，可直接运行，也可以单独调用类，设置参数然后获取数据

#include “kalman.h“
#include 
#include 
#include 
#include 

Kalman::Kalman（）
{
    m_dQ = 0.0;
    m_dR = 0.0;
    // 初始化容器大小
    m_vecSysNoise.resize（N）;
    m_vecObserNoise.resize（N）;
    m_vecReal.resize（N）;
    m_vecObser.resize（N）;
    m_vecKF.resize（N）;
    m_vecCov.resize（N）;
}

void Kalman::setIniVal（float dval float dQ float dR）
{
    m_dQ = dQ;
    m_dR = dR;
    m_vecReal[0] = dval;
    m_vecObser[0] = dval;
    m_vecKF[0] = dval;

    // 初始化系统噪声
    for（int i = 0; i < N; ++i）
    {
        m_vecSysNoise[i] = sqrt（m_dQ） * frand（）;
    }

    // 初始化观测噪声
    for（int i = 0; i < N; ++i）
    {
        m_vecObserNoise[i] = sqrt（m_dR） * frand（）;
    }

    // 协方差赋初值
    m_vecCov[1] = 0.01;
}

void Kalman::getData（vector &vecReal
                     vector &vecObserver
                     vector &vecFilter）
{
    float dXPre = 0.0;   // 一步预测值
    float dPpre = 0.0;   // 协方差一步预测
    float Kg = 0.0;      // 滤波增益

    for（int i = 1; i < N; ++i）
    {
        m_vecReal[i] = m_vecReal[i-1] + m_vecSysNoise[i-1]; // 真实温度波动变化
        m_vecObser[i] = m_vecReal[i] + m_vecObserNoise[i];  // 观测值波动变化
        // 以下五步为Kalman核心步骤
        dXPre = m_vecKF[i-1]; // 一步预测
        dPpre = m_vecCov[i-1] + m_dQ;  // 协方差一步预测
        Kg = dPpre / （dPpre + m_dR）;   // 计算增益
        m_vecKF[i] = dXPre + Kg * （m_vecObser[i] - dXPre）; // 状态更新
        m_vecCov[i] = （1 - Kg） * dPpre;  // 协方差更新
    }
    // 输出结果
    vecReal = m_vecReal;
    vecObserver = m_vecObser;
    vecFilter = m_vecKF;
    return;
}

void Kalman::displayerror（）
{
    float ObError = 0.0;   // 观测误差
    float KfError = 0.0;   // 卡尔曼滤波误差

    for（int i = 0; i < N; ++i）
    {
        cout <<“ Real Value “<        cout <<“ Observer Value “<        cout <<“ Error “<        cout <<“ KF Value “<        cout <<“ Error “<        ObError += fabs（m_vecReal[i] - m_vecObser[i]）;
        KfError += fabs（m_vecReal[i] - m_vecKF[i]）;
    }

    cout<<“\n“<<“ Observer Error “<    cout<<“ KalmanFilter Error “<}



float Kalman::frand（）
{
    static int seed = 0;
    int i = time（0） % 100000;
    seed += i;
    srand（seed）;
    float a =  2 * （（rand（） / （float）RAND_MAX） - 0.5）;//随机噪声
    return a;
}

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        2774  2019-06-29 15:59  kalman.cpp
     文件        1261  2019-06-29 16:01  kalman.h
     文件         288  2019-06-29 15:53  main.cpp
     文件         148  2019-06-28 21:11  KalmanFilter.pro