|
|
本帖最后由 Kxuan163 于 2021-6-2 13:16 編輯
例如以下例子，沒有矩陣運算：
main.c
#include "stm32f10x.h"
#include "delay.h"
#include "sys.h"
#include "adc.h"
#include "MCP4725.h"
#include "myiic.h"
#include <stdio.h>
#include "kalman.h"
#include "math.h"
u16 data; //ADC采樣值
u16 data2; //濾波所得最優(yōu)值
float MV; //浮點數(shù)格式的采樣值，即測量值
float KG; //KalmanGain 卡爾曼增益
float EC =0.1; //EstimateCovariance 估計協(xié)方差初值
float MC =0.2; //MeasureCovariance 測量協(xié)方差初值
float EV=1000; //EstimateValue 估計初值
//卡爾曼濾波函數(shù)/////////////////
float kalman_Filter()
{
data =ADC_GetConversionValue(ADC1); //采樣
MV =(float)data;
KG =EC*sqrt(1/(EC*EC+MC*MC)); //計算卡爾曼增益
EV =EV +KG*(MV-EV); //計算本次估計值
EC =sqrt(1-KG)*EC; //更新估計協(xié)方差
MC =sqrt(1-KG)*MC; //更新測量協(xié)方差
return EV; //返回最優(yōu)估計值
}
//主函數(shù)////////////////////////////////
int main()
{
delay_init();
ADC1_GPIO_Config();
ADC_Config();
MCP4725_Init();
while(1)
{
kalman_Filter(); //卡爾曼濾波
data2 =(u16)EV;
MCP4725_WriteData_Digital(data2); //濾波后再作DA變換
}
}
|
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
