卡尔曼滤波--使用心得与体会与C代码块

最新推荐文章于 2024-08-07 17:33:57 发布

cap reagan

最新推荐文章于 2024-08-07 17:33:57 发布

阅读量178

点赞数 9

文章标签： c语言算法开发语言

本文链接：https://blog.csdn.net/ligenshuai/article/details/137691635

版权

卡尔曼滤波是一种观测器，他的核心在于对测量值与估计值进行数据融合。
关于内容和推导最后面会给出参考文章和视频链接。
公式和公式融合需要借助MATLAB进行矩阵运算推导，所以感叹MAT的强大。
代码部分是很简单的一个流水模型，称重和流量作为数据融合的两个参考。

一、直接给代码

1.欢迎批评指正

代码如下：

#include <iostream>
#include <stdio.h>
#include "stdlib.h"
#include <string.h>
using namespace std;

double spray_weightsensor_val;//测量称重
double weight_mea = 0.0;//输入称重

double rate[2] = {0.0};//测量流量
double fr_mea = 0.0;//输入流量

double weight_k_pre = 0.0;//先验称重预测
double weight_k_est = 0.0;//后验称重估计
double q_k_pre = 0.0;//先验流量预测
double q_k_est = 0.0;//后验流量估计

double dt = 1.0;
double W1 = 0.0;//过程噪声
/* A[][] = {1 , -dt
            0 ,  1} */
double zk_we = 0.0;//观测值
double zk_fr = 0.0;//观测值
double Hat[2][2] = {1.0f,0.0,
                    0.0,1.0f};

double V_1K = 0.0;//观测噪声
double V_2K = 0.0;//观测噪声

double Pk_pre[2][2] = {0,0,
                       0,0};//先验误差方差

double Pk_est[2][2] = {0,0,
                       0,0};//更新误差方差
/* 超参数 */
double Q[2][2] = {1,0,
                  0,1};//过程噪声方差=协方差矩阵
double R[2][2] = {1,0,
                  0,1};//观测噪声方差=协方差矩阵

double K_kal[2][2] = {0,0,
                      0,0};//卡尔曼增益
double Jes = 0.0;//Kalman增益分母简化 

int main()
{
    /* 传感器测量数据 */
    /* 200ms进入一次 */
    fr_mea = (rate[0] + rate[1]) /  60.0 / 5.0 / 2;// g/200ms / 2 
    weight_mea = spray_weightsensor_val;//0：传感器观测值
    
    /* ==============预测============== */
    /* 1:先验估计 */
    weight_k_pre = weight_k_est - (dt * q_k_est) + W1; 
    q_k_pre = q_k_est + W1;

    zk_we = weight_mea * Hat[0][0] + V_1K;//Z_1k
    zk_fr = fr_mea * Hat[0][0] + V_2K; //Z_2k

    /* 2:先验误差的协方差矩阵 = 误差方差 */
    Pk_pre[0][0] = Pk_est[0][0] - dt * Pk_est[1][0] - dt * Pk_est[0][1] + dt*dt * Pk_est[1][1] + Q[0][0];  //P0
    Pk_pre[0][1] = Pk_est[0][1] - dt * Pk_est[1][1]; //P1
    Pk_pre[1][0] = Pk_est[1][0] - dt * Pk_est[1][1]; //P2
    Pk_pre[1][1] = Pk_est[1][1] + Q[1][1]; //P3
    /* ================矫正=============== */
    /* 3：求卡尔曼增益 */
    Jes = Pk_pre[0][0]*Pk_pre[1][1] - Pk_pre[0][1] * Pk_pre[1][0] + Pk_pre[0][0] * R[1][1] + Pk_pre[1][1]*R[0][0] + R[0][0] * R[1][1];
    K_kal[0][0] = (Pk_pre[0][0]*(Pk_pre[1][1]+R[1][1]) - Pk_pre[0][1]*Pk_pre[1][0])/Jes;
    K_kal[0][1] = (Pk_pre[0][1]*(Pk_pre[0][0]+R[0][0]) - Pk_pre[0][0]*Pk_pre[0][1])/Jes;
    K_kal[1][0] = (Pk_pre[1][0]*(Pk_pre[1][1]+R[1][1]) - Pk_pre[1][0]*Pk_pre[1][1])/Jes;
    K_kal[1][1] = (Pk_pre[1][1]*(Pk_pre[0][0]+R[0][0]) - Pk_pre[0][1]*Pk_pre[1][0])/Jes;
    /* 4：后验估计 */
    weight_k_est = weight_k_pre + K_kal[0][0]*(zk_we-Hat[0][0]*weight_k_pre)+K_kal[0][1]*(zk_fr-Hat[1][1]*q_k_pre);
    q_k_est = q_k_pre + K_kal[1][0] * (zk_we - Hat[0][0]*weight_k_pre) + K_kal[1][1]*(zk_fr-Hat[1][1]*q_k_pre);
    /* 5: 更新误差的协方差矩阵 */
    Pk_est[0][0] = 0 - Pk_pre[0][0]*(Hat[0][0]*K_kal[0][0] - 1) - Hat[1][1] * K_kal[0][1] * Pk_pre[1][0];
    Pk_est[0][1] = 0 - Pk_pre[0][1]*(Hat[0][0]*K_kal[0][0] - 1) - Hat[1][1] * K_kal[0][1] * Pk_pre[1][1];
    Pk_est[1][0] = 0 - Pk_pre[1][0]*(Hat[1][1]*K_kal[1][1] - 1) - Hat[0][0] * K_kal[1][0] * Pk_pre[0][0];
    Pk_est[1][1] = 0 - Pk_pre[1][1]*(Hat[1][1]*K_kal[1][1] - 1) - Hat[0][0] * K_kal[1][0] * Pk_pre[0][1];




    return 0;
}