当前位置：首页 > article >正文

卡尔曼滤波效果(python应用)

article 2024/12/21 17:28:33

在这里插入图片描述

卡尔曼三大公式:
1.卡尔曼增益k(卡尔曼核心公式) = (上一次估计误差)/(上一次估计误差 + 本次测量误差)
2.本次估计值 = 上一次的估计值 + 系数k(gain) * (当前测量值(Zk) - 上一次的估计值hat(x)k-1)
3.新的估计误差 = (1 - k) * 上一次估计误差

python模拟:

import random

Zk = [103,99,97,104,102,105,95,96,97,103,104,100,101,103,96,99,98,102]  #模拟真实测量的数据
Emea = 5.0         #测量误差,同一工具进行测量这个值不会变
Eesti = 3.0        #估计误差,根据经验给定
K = 0.0            #卡尔曼增益  K ∈ [0,1]
Hat_X = 103.0       #估计值,随意给定

if __name__ == "__main__":
    # for i in range(100):
    #     Zk.append(random.randint(95, 105))  #向Zk添加100个数据(数据范围100±5)
    for i in range(1,18,1):        #从1开始计算
        K = Eesti/(Emea + Eesti)   #更新卡尔曼增益
        Hat_X = Hat_X + K * (Zk[i] - Hat_X)  #更新估计值
        Eesti = (1.0 - K) * Eesti            #更新估计误差
        print(f"第{i}次 ","实际测量数据:{:.3f}".format(Zk[i])," 增益:{:.3f}".format(K)," 估计值:{:.3f}".format(Hat_X))

结果:

第1次  实际测量数据:99.000  增益:0.375  估计值:101.500
第2次  实际测量数据:97.000  增益:0.273  估计值:100.273
第3次  实际测量数据:104.000  增益:0.214  估计值:101.071
第4次  实际测量数据:102.000  增益:0.176  估计值:101.235
第5次  实际测量数据:105.000  增益:0.150  估计值:101.800
第6次  实际测量数据:95.000  增益:0.130  估计值:100.913
第7次  实际测量数据:96.000  增益:0.115  估计值:100.346
第8次  实际测量数据:97.000  增益:0.103  估计值:100.000
第9次  实际测量数据:103.000  增益:0.094  估计值:100.281
第10次  实际测量数据:104.000  增益:0.086  估计值:100.600
第11次  实际测量数据:100.000  增益:0.079  估计值:100.553
第12次  实际测量数据:101.000  增益:0.073  估计值:100.585
第13次  实际测量数据:103.000  增益:0.068  估计值:100.750
第14次  实际测量数据:96.000  增益:0.064  估计值:100.447
第15次  实际测量数据:99.000  增益:0.060  估计值:100.360
第16次  实际测量数据:98.000  增益:0.057  估计值:100.226
第17次  实际测量数据:102.000  增益:0.054  估计值:100.321

使用excel表格进行分析:

import os
import random
from os.path import dirname
import xlwt

Zk = [] #模拟真实测量值
Emeam = 5.0 #测量误差
Eesti = 5.0 #估计误差
K = 0.0 #卡尔曼增益
Hatx = 0.0 #估计值

def Find_Path_Excel(name):
    path = os.path.join(dirname(__file__), name)
    return path

if __name__ == '__main__':
    book = xlwt.Workbook(Find_Path_Excel('test.xlsx'))  # 创建Workbook，相当于创建Excel
    ws = book.add_sheet('Sheet1')
    for i in range(100):
        Zk.append(random.randint(90, 100)) #数据范围95±5
    ws.write(0, 0, '实际测量值')
    ws.write(0, 1, '估计值')
    ws.write(0, 2, '卡尔曼增益')
    ws.write(0, 3, '估计误差')
    for i in range(0,100,1):
        K = Eesti/(Eesti + Emeam)
        Hatx = Hatx + K * (Zk[i] - Hatx)
        Eesti = (1 - K) * Eesti
        # 向表中添加数据
        ws.write(i+1, 0, ("{:.2f}".format(Zk[i])))
        ws.write(i+1, 1, ("{:.2f}".format(Hatx)))
        ws.write(i+1, 2, ("{:.2f}".format(K)))
        ws.write(i+1, 3, ("{:.2f}".format(Eesti)))
    book.save('test9.xlsx')