卡尔曼滤波原理及c++实现

原创已于 2024-07-31 09:35:34 修改 · 2.2k 阅读

38 ·

CC 4.0 BY-SA版权

文章标签：

#c++ #算法 #人工智能

于 2022-12-22 16:35:08 首次发布

本文介绍卡尔曼滤波原理,一种用于处理测量噪声并估计系统状态的最优估计算法。文章详细解释了卡尔曼滤波器如何通过预测和更新步骤来估计系统状态,并讨论了其在制导、导航和计算机视觉等领域的应用。此外,还提供了使用C++实现卡尔曼滤波的代码示例,包括设置初始值、获取滤波数据和显示滤波效果等功能。

卡尔曼滤波原理及c++实现

滤波原理

卡尔曼滤波是一种最优估计算法。

用处：1）利用可测量值估算无法测量的量。2）对有测量噪声的物理量进行估计。常用于制导和导航、计算机视觉等。

状态估计器

在这里插入图片描述

卡尔曼滤波器与状态观测器的区别

状态观测器是针对确定性系统，卡尔曼滤波器是针对随机系统。

卡尔曼滤波的五个公式

预测公式
$x^k=Fkxk−1^+BkukPk′=APk−1AT+Qk \hat{x}_k=F_k\hat{x_{k-1}}+B_ku_k\\ P_k'=AP_{k-1}A^T+Q_k\\$
式中， $x^k\hat x_k$ 为系统状态估计值。 $F_k$ 为系统状态变换方程得到的状态转移矩阵，也叫预测矩阵。

更新步骤：
$K′=PkHkT(HkPkHkT+Rk)−1x^k′=x^k+K′(zk⃗−Hkx^k)Pk′=Pk−K′HkPk=(I−K′Hk)Pk K'=P_kH_k^T(H_kP_kH_k^T+R_k)^{-1}\\ \hat x_k'=\hat x_k+K'(\vec {z_k}-H_k\hat x_k)\\ P_k'=P_k-K'H_kP_k=(I-K'H_k)P_k$
在这里插入图片描述

扩展卡尔曼滤波器

对于非线性系统，高斯分布的噪声在经过非线性变换后可能不呈高斯分布，导致卡尔曼滤波不收敛。因此，需要用到扩展卡尔曼滤波。

在这里插入图片描述

C++代码实现

#include <vector>
#include<iostream>
#include <math.h>
#include <cstdlib>
#include <iomanip>

#define N 50
using namespace std;
class Kalman
{
public:
    Kalman();
    /**
     * @brief setIniVal 设置初始值
     * @param dval 待测数据初始值
     * @param dQ   系统噪声方差(有默认值)
     * @param dR   观测噪声方差(有默认值)
     */
    void setIniVal(float dval, float dQ = 0.01, float dR = 0.25);

    /**
     * @brief getData 获取滤波数据
     * @param vecReal 真实值
     * @param vecObserver 观测值
     * @param vecFilter 滤波后的值
     */
    void getData(vector<float>& vecReal,
        vector<float>& vecObserver,
        vector<float>& vecFilter);
    // 显示滤波效果
    void displayError();

private:
    // 产生-1与1之间的随机数
    float frand();


private:
    float m_dQ;       // 系统噪声方差
    float m_dR;       // 观测噪声方差
    vector<float> m_vecSysNoise;   // 系统噪声
    vector<float> m_vecObserNoise; // 观测噪声
    vector<float> m_vecReal;   // 真实值
    vector<float> m_vecObser;  // 观测值
    vector<float> m_vecKF;     // 滤波值
    vector<float> m_vecCov;    // 协方差

};





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<float>& vecReal,
    vector<float>& vecObserver,
    vector<float>& 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 " << setprecision(5) << m_vecReal[i];
        cout << " Observer Value " << setprecision(5) << m_vecObser[i];
        cout << " Error " << fabs(m_vecObser[i] - m_vecReal[i]);
        cout << " KF Value " << setprecision(5) << m_vecKF[i];
        cout << " Error " << fabs(m_vecKF[i] - m_vecReal[i]) << "\n";
        ObError += fabs(m_vecReal[i] - m_vecObser[i]);
        KfError += fabs(m_vecReal[i] - m_vecKF[i]);
    }

    cout << "\n" << " Observer Error " << setprecision(5) << ObError << "\n";
    cout << " KalmanFilter Error " << setprecision(5) << KfError;
}



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;
}

//主函数调用
int main()
{

    Kalman kal;
    vector<float> vecReal;
    vector<float> vecOb;
    vector<float> vecKF;

    kal.setIniVal(30);
    kal.getData(vecReal, vecOb, vecKF);
    kal.displayError();
    return 0;
}

Kalman kal;
vector<float> vecReal;
vector<float> vecOb;
vector<float> vecKF;

kal.setIniVal(30);
kal.getData(vecReal, vecOb, vecKF);
kal.displayError();
return 0;
}