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

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

滤波原理

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

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

状态估计器

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

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

卡尔曼滤波的五个公式

预测公式
$$\begin{gathered} {\hat{x}}_k=F_k\widehat{x_{k-1}}+B_k{u}_k \\ {P}_k^{\prime }=A{P}_{k-1}{A}^T+Q_k \end{gathered}$$
式中，${\hat{x}}_k$为系统状态估计值。$F_k$为系统状态变换方程得到的状态转移矩阵，也叫预测矩阵。

更新步骤：
$$\begin{gathered} K^{\prime }=P_k{H}_k^T(H_k{P}_k{H}_k^T+R_k{)}^{-1} \\ {\hat{x}}_k^{\prime }={\hat{x}}_k+K^{\prime }(\overrightarrow{z_k}-H_k{\hat{x}}_k) \\ {P}_k^{\prime }=P_k-K^{\prime }{H}_k{P}_k=(I-K^{\prime }{H}_k)P_k \end{gathered}$$

扩展卡尔曼滤波器

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

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;

}