卡爾曼濾波算法程序,輸入隨機數，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;
}

ID:640288 · 發表于 2019-11-12 11:10

您好，這個程序中您說的過程測量噪聲是實驗測的，能給點測量方法嗎，最近在做卡爾曼濾波算法對系統誤差測量一籌莫展。

ID:640288 · 發表于 2019-11-12 11:15

您好，我想請問一下系統測量誤差實驗測得的可以分享一下實驗測量的步驟嗎？

帳號		自動登錄	找回密碼
密碼			立即注冊

欧美极品高清xxxxhd,国产日产欧美最新,无码AV国产东京热AV无码,国产精品人与动性XXX,国产传媒亚洲综合一区二区,四库影院永久国产精品,毛片免费免费高清视频,福利所导航夜趣136

卡爾曼濾波算法程序,輸入隨機數，Q和R自己選

評分