热门标签
热门文章
- 1Spring Security 权限控制_springsecurity权限控制
- 2【华为OD机试真题 Java语言】455、螺旋数字矩阵 | 机试真题+思路参考+代码解析（C卷）_华为od机试 - 螺旋数字矩阵
- 3Uni-app开发介绍及入门_uniapp开发
- 4简单的.bat恶搞代码_bat文件好玩的命令
- 5若依框架下载文件_若依文件下载
- 6GPT-3：NLP领域的革新者
- 7从零实现诗词GPT大模型：GPT是怎么生成内容的？
- 8SQL入门基本语句练习_sql语句练习
- 9应用开发平台业务支撑功能——单据编号、流水号功能方案、设计与实现_spring boot 组件 单据编号
- 10git上传仓库内容超过100M解决方法git_git超过100m
当前位置: article > 正文
卡尔曼滤波算法 C语言实现 示例_卡尔曼滤波c语言实现
作者：Gausst松鼠会 | 2024-04-26 23:44:12
赞
踩
卡尔曼滤波c语言实现
1.概念
卡尔曼滤波（Kalman filtering）是一种利用
| k时刻 状态预测值（先验估计值） | ||
| k-1时刻 状态最优估计值（后验估计值） | ||
| k时刻 状态预测协方差（先验预测协方差） | 真实值与预测值之间的协方差 | |
| k时刻 状态最优估计协方差（后验最优估计协方差） | 真实值与最优估计值之间的协方差 | |
| k-1时刻 状态最优估计协方差（后验最优估计协方差） | ||
| 过程激励噪声协方差 | ||
| 测量噪声协方差 | ||
| k时刻 卡尔曼增益 | ||
4.卡尔曼滤波 代码示例
4.1.卡尔曼滤波 C语言代码示例
4.1.1. 卡尔曼滤波 五大公式简化
| k时刻 状态输入控制量 | 无输入控制量，可以为 0 | |
| 状态转移矩阵 | 以1维数据为例，可以为 1 | |
| 控制输入矩阵 | 以1维数据为例，可以为 1 | |
| 状态观测矩阵 | 以1维数据为例，可以为 1 |
===>
（1）
===>
（2）
===>
（3）
===>
（4）
===>
（5）
卡尔曼简化后的公式：
（2）
（3）
（4）
（5）
4.1.2.卡尔曼滤波 C语言代码
卡尔曼函数、调试main函数：
- #include <stdio.h> //库头文件
-
- #define TEST_PULSE_BUF_LEN 800
- extern float testPulseBuf[];
-
-
- //1. 结构体类型定义
- typedef struct
- {
- float P; //估算协方差
- float G; //卡尔曼增益
- float Q; //过程噪声协方差,Q增大，动态响应变快，收敛稳定性变坏
- float R; //测量噪声协方差,R增大，动态响应变慢，收敛稳定性变好
- float Output; //卡尔曼滤波器输出
- }KFPTypeS; //Kalman Filter parameter type Struct
-
-
- //2.定义卡尔曼结构体参数并初始化
- KFPTypeS kfpVar =
- {
- 20, //估算协方差. 初始化值为 0.02
- 0, //卡尔曼增益. 初始化值为 0
- 1, //过程噪声协方差,Q增大，动态响应变快，收敛稳定性变坏. 初始化值为 0.001
- 1000, //测量噪声协方差,R增大，动态响应变慢，收敛稳定性变好. 初始化值为 1
- 930 //卡尔曼滤波器输出. 初始化值为 0
- };
-
- /**
- ******************************************************************************
- * @brief 卡尔曼滤波器 函数
- * @param *kfp - 卡尔曼结构体参数
- * @param input - 需要滤波的参数的测量值（即传感器的采集值）
- * @return 卡尔曼滤波器输出值（最优值）
- * @note
- ******************************************************************************
- */
- float KalmanFilter(KFPTypeS *kfp, float input)
- {
- //估算协方差方程：当前 估算协方差 = 上次更新 协方差 + 过程噪声协方差
- kfp->P = kfp->P + kfp->Q;
-
- //卡尔曼增益方程：当前 卡尔曼增益 = 当前 估算协方差 / （当前 估算协方差 + 测量噪声协方差）
- kfp->G = kfp->P / (kfp->P + kfp->R);
-
- //更新最优值方程：当前 最优值 = 当前 估算值 + 卡尔曼增益 * （当前 测量值 - 当前 估算值）
- kfp->Output = kfp->Output + kfp->G * (input - kfp->Output); //当前 估算值 = 上次 最优值
-
- //更新 协方差 = （1 - 卡尔曼增益） * 当前 估算协方差。
- kfp->P = (1 - kfp->G) * kfp->P;
-
- return kfp->Output;
- }
-
- /**
- ******************************************************************************
- * @brief 主函数
- * @param None
- * @return None
- * @note
- ******************************************************************************
- */
- void main(void)
- {
- for(int i=0; i<TEST_PULSE_BUF_LEN; i++)
- {
- printf("%4d\r\n", (int)KalmanFilter(&kfpVar, testPulseBuf[i])); //打印
- }
- }
测试的原始数据：
- float testPulseBuf1[800] =
- {
- 939
- ,1032
- ,1059
- ,1031
- ,898
- ,849
- ,878
- ,993
- ,991
- ,1010
- ,975
- ,1151
- ,1027
- ,902
- ,977
- ,996
- ,1069
- ,1140
- ,900
- ,907
- ,1202
- ,1004
- ,1027
- ,994
- ,883
- ,827
- ,1055
- ,1017
- ,977
- ,1020
- ,823
- ,977
- ,816
- ,1047
- ,938
- ,1044
- ,891
- ,985
- ,825
- ,1036
- ,1283
- ,940
- ,934
- ,1004
- ,951
- ,1027
- ,922
- ,710
- ,1016
- ,930
- ,1149
- ,968
- ,1013
- ,1161
- ,985
- ,1013
- ,1249
- ,1035
- ,1147
- ,1072
- ,954
- ,1000
- ,943
- ,991
- ,1120
- ,1003
- ,956
- ,988
- ,1107
- ,984
- ,975
- ,989
- ,1042
- ,782
- ,973
- ,820
- ,1123
- ,859
- ,930
- ,1147
- ,1015
- ,916
- ,1122
- ,949
- ,999
- ,768
- ,851
- ,1085
- ,986
- ,904
- ,975
- ,969
- ,1146
- ,952
- ,1066
- ,845
- ,1135
- ,1002
- ,976
- ,957
- ,912
- ,928
- ,848
- ,907
- ,1035
- ,997
- ,898
- ,1006
- ,1132
- ,920
- ,901
- ,925
- ,1073
- ,987
- ,877
- ,936
- ,959
- ,1095
- ,1065
- ,927
- ,1003
- ,786
- ,1049
- ,1038
- ,1010
- ,1007
- ,900
- ,995
- ,1042
- ,1101
- ,1209
- ,963
- ,883
- ,1051
- ,979
- ,1191
- ,961
- ,1006
- ,1085
- ,1058
- ,926
- ,983
- ,974
- ,1157
- ,929
- ,1171
- ,1105
- ,1031
- ,1036
- ,847
- ,1033
- ,928
- ,1198
- ,1270
- ,1021
- ,915
- ,1071
- ,1093
- ,897
- ,958
- ,959
- ,1017
- ,1135
- ,1027
- ,963
- ,1093
- ,1080
- ,1063
- ,951
- ,885
- ,997
- ,930
- ,927
- ,937
- ,1073
- ,1073
- ,1017
- ,1080
- ,1125
- ,1058
- ,951
- ,979
- ,785
- ,1076
- ,702
- ,1131
- ,952
- ,1120
- ,1314
- ,965
- ,959
- ,890
- ,1103
- ,1058
- ,805
- ,1034
- ,923
- ,1114
- ,1005
- ,895
- ,986
- ,913
- ,1075
- ,1036
- ,1062
- ,975
- ,1095
- ,987
- ,892
- ,995
- ,1022
- ,1034
- ,819
- ,1071
- ,964
- ,998
- ,961
- ,875
- ,913
- ,849
- ,799
- ,1069
- ,1108
- ,1075
- ,1014
- ,987
- ,1009
- ,946
- ,1186
- ,1037
- ,833
- ,816
- ,742
- ,982
- ,952
- ,1045
- ,771
- ,1032
- ,1080
- ,1218
- ,1024
- ,950
- ,992
- ,933
- ,973
- ,880
- ,787
- ,1023
- ,894
- ,955
- ,1088
- ,928
- ,1031
- ,891
- ,760
- ,1107
- ,1007
- ,1028
- ,1017
- ,1063
- ,1118
- ,902
- ,959
- ,983
- ,915
- ,956
- ,1150
- ,1213
- ,1090
- ,1113
- ,944
- ,972
- ,978
- ,776
- ,937
- ,1184
- ,890
- ,924
- ,1152
- ,962
- ,1041
- ,1024
- ,792
- ,1070
- ,1075
- ,1145
- ,1093
- ,898
- ,843
- ,897
- ,862
- ,998
- ,864
- ,1116
- ,990
- ,1140
- ,916
- ,827
- ,1082
- ,824
- ,1079
- ,1008
- ,1043
- ,825
- ,811
- ,1134
- ,967
- ,1007
- ,862
- ,892
- ,1003
- ,1013
- ,1008
- ,1025
- ,1099
- ,958
- ,943
- ,924
- ,956
- ,1025
- ,968
- ,981
- ,1182
- ,919
- ,997
- ,926
- ,1008
- ,982
- ,1062
- ,1011
- ,944
- ,956
- ,851
- ,1155
- ,952
- ,939
- ,1052
- ,1002
- ,1011
- ,1034
- ,1079
- ,1131
- ,906
- ,1029
- ,976
- ,833
- ,1046
- ,1206
- ,953
- ,1168
- ,789
- ,899
- ,1039
- ,856
- ,801
- ,850
- ,814
- ,784
- ,975
- ,952
- ,1029
- ,881
- ,955
- ,1011
- ,1066
- ,1120
- ,987
- ,1036
- ,919
- ,993
- ,1123
- ,851
- ,1155
- ,967
- ,902
- ,1086
- ,970
- ,936
- ,926
- ,895
- ,998
- ,1120
- ,1084
- ,900
- ,968
- ,1110
- ,1153
- ,1082
- ,1071
- ,895
- ,845
- ,1048
- ,986
- ,987
- ,1073
- ,1059
- ,924
- ,1170
- ,952
- ,849
- ,950
- ,1014
- ,992
- ,920
- ,999
- ,1057
- ,986
- ,964
- ,1092
- ,878
- ,1009
- ,908
- ,1072
- ,1112
- ,1038
- ,1017
- ,1074
- ,799
- ,968
- ,909
- ,948
- ,1000
- ,848
- ,771
- ,1014
- ,1091
- ,960
- ,1074
- ,1061
- ,1134
- ,968
- ,1187
- ,1029
- ,938
- ,1098
- ,1015
- ,942
- ,855
- ,964
- ,1024
- ,892
- ,1119
- ,905
- ,956
- ,859
- ,889
- ,1017
- ,1174
- ,825
- ,1097
- ,973
- ,927
- ,1112
- ,1018
- ,984
- ,1098
- ,995
- ,932
- ,944
- ,891
- ,930
- ,921
- ,936
- ,876
- ,1042
- ,856
- ,1085
- ,856
- ,1108
- ,990
- ,834
- ,1091
- ,1059
- ,943
- ,1081
- ,1107
- ,1029
- ,884
- ,1074
- ,1028
- ,858
- ,961
- ,991
- ,954
- ,1020
- ,1008
- ,941
- ,1020
- ,1077
- ,886
- ,946
- ,1190
- ,1107
- ,968
- ,1287
- ,982
- ,995
- ,1050
- ,1015
- ,970
- ,936
- ,1156
- ,1026
- ,1023
- ,988
- ,930
- ,971
- ,1081
- ,916
- ,958
- ,1206
- ,994
- ,799
- ,915
- ,1056
- ,1041
- ,1081
- ,1017
- ,927
- ,1111
- ,932
- ,931
- ,1057
- ,1118
- ,1069
- ,929
- ,948
- ,876
- ,1059
- ,837
- ,1166
- ,1115
- ,955
- ,920
- ,1109
- ,993
- ,1066
- ,793
- ,782
- ,886
- ,982
- ,1065
- ,970
- ,962
- ,1015
- ,1200
- ,902
- ,877
- ,937
- ,927
- ,1028
- ,1095
- ,1057
- ,1067
- ,975
- ,986
- ,985
- ,884
- ,1051
- ,992
- ,750
- ,1079
- ,982
- ,918
- ,1073
- ,823
- ,1064
- ,941
- ,797
- ,895
- ,933
- ,1126
- ,804
- ,1133
- ,1091
- ,1006
- ,1085
- ,974
- ,992
- ,1015
- ,889
- ,1120
- ,1079
- ,982
- ,1109
- ,1038
- ,1024
- ,832
- ,890
- ,917
- ,1059
- ,798
- ,924
- ,1048
- ,1016
- ,915
- ,933
- ,934
- ,843
- ,1161
- ,902
- ,1018
- ,1063
- ,931
- ,1280
- ,936
- ,866
- ,915
- ,1051
- ,888
- ,1275
- ,1108
- ,1015
- ,978
- ,907
- ,873
- ,944
- ,1119
- ,1015
- ,969
- ,1103
- ,1092
- ,1066
- ,997
- ,1123
- ,929
- ,1188
- ,1108
- ,1032
- ,881
- ,948
- ,987
- ,940
- ,978
- ,1119
- ,1104
- ,998
- ,967
- ,998
- ,1077
- ,1025
- ,1056
- ,994
- ,922
- ,889
- ,1022
- ,982
- ,1147
- ,885
- ,1031
- ,731
- ,870
- ,930
- ,1116
- ,897
- ,1045
- ,1097
- ,1021
- ,1120
- ,944
- ,887
- ,967
- ,1176
- ,1088
- ,955
- ,836
- ,1070
- ,1063
- ,1176
- ,1091
- ,890
- ,852
- ,954
- ,1117
- ,1010
- ,1064
- ,923
- ,883
- ,879
- ,790
- ,883
- ,974
- ,1101
- ,851
- ,912
- ,1034
- ,1057
- ,952
- ,982
- ,1052
- ,1105
- ,999
- ,1187
- ,1093
- ,962
- ,1053
- ,1201
- ,1202
- ,1037
- ,929
- ,926
- ,897
- ,1134
- ,970
- ,1070
- ,961
- ,1038
- ,1088
- ,1018
- ,904
- ,883
- ,1000
- ,1063
- ,913
- ,1044
- ,1020
- ,1104
- ,1029
- ,1030
- ,1038
- ,894
- ,1001
- ,1228
- ,871
- ,982
- ,836
- ,1051
- ,1029
- ,1067
- ,1146
- ,976
- ,1000
- ,995
- ,1061
- ,1004
- ,882
- ,1053
- ,932
- ,1031
- ,861
- ,1111
- ,1144
- ,1034
- ,987
- ,1142
- ,1212
- ,971
- ,1029
- ,1016
- ,1051
- ,1153
- ,1071
- ,817
- ,1103
- ,1007
- ,1010
- ,1008
- ,1149
- ,914
- ,1006
- ,950
- ,986
- ,912
- ,1035
- ,914
- ,1160
- ,884
- ,1154
- ,990
- ,1013
- ,1014
- ,949
- ,978
- ,866
- ,894
- ,836
- ,1096
- ,967
- ,1029
- ,1017
- ,894
- ,1119
- ,1026
- ,1073
- ,929
- ,799
- ,1126
- ,941
- ,1044
- ,993
- ,991
- ,1093
- ,1007
- ,782
- ,940
- ,1081
- ,773
- ,882
- ,918
- ,941
- };
卡尔曼滤波结果：
4.2.卡尔曼滤波 Python语言代码示例
这里真实值为 x=-0.377，且假设A=1， H=1
观测值存在噪声，那么如何估计出实际的值呢？
这里给出两种方案，一种是北卡大学开源的，直接通过公式计算的结果
- # -*- coding=utf-8 -*-
- # Kalman filter example demo in Python
-
- # A Python implementation of the example given in pages 11-15 of "An
- # Introduction to the Kalman Filter" by Greg Welch and Gary Bishop,
- # University of North Carolina at Chapel Hill, Department of Computer
- # Science, TR 95-041,
- # http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html
-
- # by Andrew D. Straw
- #coding:utf-8
- import numpy
- import pylab
-
- #这里是假设A=1，H=1的情况
-
- # 参数初始化
- n_iter = 50
- sz = (n_iter,) # size of array
- x = -0.37727 # 真实值
- z = numpy.random.normal(x,0.1,size=sz) # 观测值 ,观测时存在噪声
-
- Q = 1e-5 # process variance
-
- # 分配数组空间
- xhat=numpy.zeros(sz) # x 滤波估计值
- P=numpy.zeros(sz) # 滤波估计协方差矩阵
- xhatminus=numpy.zeros(sz) # x 估计值
- Pminus=numpy.zeros(sz) # 估计协方差矩阵
- K=numpy.zeros(sz) # 卡尔曼增益
-
- R = 0.1**2 # estimate of measurement variance, change to see effect
-
- # intial guesses
- xhat[0] = 0.0
- P[0] = 1.0
-
- for k in range(1,n_iter):
- # 预测
- xhatminus[k] = xhat[k-1] #X(k|k-1) = AX(k-1|k-1) + BU(k) + W(k),A=1,BU(k) = 0
- Pminus[k] = P[k-1]+Q #P(k|k-1) = AP(k-1|k-1)A' + Q(k) ,A=1
-
- # 更新
- K[k] = Pminus[k]/( Pminus[k]+R ) #Kg(k)=P(k|k-1)H'/[HP(k|k-1)H' + R],H=1
- xhat[k] = xhatminus[k]+K[k]*(z[k]-xhatminus[k]) #X(k|k) = X(k|k-1) + Kg(k)[Z(k) - HX(k|k-1)], H=1
- P[k] = (1-K[k])*Pminus[k] #P(k|k) = (1 - Kg(k)H)P(k|k-1), H=1
-
- pylab.figure()
- pylab.plot(z,'k+',label='noisy measurements') #观测值
- pylab.plot(xhat,'b-',label='a posteri estimate') #滤波估计值
- pylab.axhline(x,color='g',label='truth value') #真实值
- pylab.legend()
- pylab.xlabel('Iteration')
- pylab.ylabel('Voltage')
-
- pylab.figure()
- valid_iter = range(1,n_iter) # Pminus not valid at step 0
- pylab.plot(valid_iter,Pminus[valid_iter],label='a priori error estimate')
- pylab.xlabel('Iteration')
- pylab.ylabel('$(Voltage)^2$')
- pylab.setp(pylab.gca(),'ylim',[0,.01])
- pylab.show()
另外一种是调用
from filterpy.kalman 里的卡尔曼滤波函数
- from filterpy.kalman import KalmanFilter
- import numpy as np
- np.random.seed(0)
- kf = KalmanFilter(dim_x=1, dim_z=1)
- kf.F = np.array([1])
- kf.H = np.array([1])
- kf.R = np.array([0.1**2])
- kf.P = np.array([1.0])
- kf.Q = 1e-5
- xhat[0] = 0.0
- P[0] = 1.0
- for k in range(1,n_iter):
- kf.predict()
- xhat[k] = kf.x
- kf.update(z[k], 0.1**2, np.array([1]))
-
- pylab.figure()
- pylab.plot(z,'k+',label='noisy measurements') #观测值
- pylab.plot(xhat,'b-',label='a posteri estimate') #滤波估计值
- pylab.axhline(x,color='g',label='truth value') #真实值
- pylab.legend()
- pylab.xlabel('Iteration')
- pylab.ylabel('Voltage')
-
- pylab.figure()
- valid_iter = range(1,n_iter) # Pminus not valid at step 0
- pylab.plot(valid_iter,Pminus[valid_iter],label='a priori error estimate')
- pylab.xlabel('Iteration')
- pylab.ylabel('$(Voltage)^2$')
- pylab.setp(pylab.gca(),'ylim',[0,.01])
- pylab.show()
python-opencv中的卡尔曼滤波函数
- kalman = cv2.KalmanFilter(1, 1)
- kalman.transitionMatrix = np.array([[1]], np.float32) # 转移矩阵 A
- kalman.measurementMatrix = np.array([[1]], np.float32) # 测量矩阵 H
- kalman.measurementNoiseCov = np.array([[1]], np.float32) * 0.01 # 测量噪声 R
- kalman.processNoiseCov = np.array([[1]], np.float32) * 1e-5 # 过程噪声 Q
- kalman.errorCovPost = np.array([[1.0]], np.float32) # 最小均方误差 P
-
- xhat = np.zeros(sz) # x 滤波估计值
- kalman.statePost = np.array([xhat[0]], np.float32)
- for k in range(1, n_iter):
- # print(np.array([z[k]], np.float32))
- mes = np.reshape(np.array([z[k]], np.float32), (1, 1))
- # # print(mes.shape)
- xhat[k] = kalman.predict()
- kalman.correct(np.array(mes, np.float32))
-
-
- pylab.figure()
- pylab.plot(z, 'k+', label='noisy measurements') # 观测值
- pylab.plot(xhat, 'b-', label='a posteri estimate') # 滤波估计值
- pylab.axhline(x, color='g', label='truth value') # 真实值
- pylab.legend()
- pylab.xlabel('Iteration')
- pylab.ylabel('Voltage')
- pylab.show()
三者都能得到同一结果
声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/Gausst松鼠会/article/detail/493716
推荐阅读
相关标签
