卡尔曼滤波算法 C语言实现示例

卡尔曼函数与调试main函数。

匠在江湖

8730人浏览 · 2023-05-31 15:46:02

匠在江湖 · 2023-05-31 15:46:02 发布

1.概念

卡尔曼滤波（Kalman filtering）是一种利用

${\hat{X}}'_{k}$	k时刻状态预测值（先验估计值）
$\hat{X}_{k-1}$	k-1时刻状态最优估计值（后验估计值）
${P}'_{k}$	k时刻状态预测协方差（先验预测协方差）	真实值与预测值之间的协方差
$P_{k}$	k时刻状态最优估计协方差（后验最优估计协方差）	真实值与最优估计值之间的协方差
$P_{k-1}$	k-1时刻状态最优估计协方差（后验最优估计协方差）
$Q$	过程激励噪声协方差
$R$	测量噪声协方差
$G_{k}$	k时刻卡尔曼增益
$A^{T}$
$H^{T}$

4.卡尔曼滤波代码示例

4.1.卡尔曼滤波 C语言代码示例

4.1.1. 卡尔曼滤波五大公式简化

$U_{k}$	k时刻状态输入控制量	无输入控制量，可以为 0
$A$	状态转移矩阵	以1维数据为例，可以为 1
$B$	控制输入矩阵	以1维数据为例，可以为 1
$H$	状态观测矩阵	以1维数据为例，可以为 1

${\hat{X}}'_{k} = 1*\hat{X}_{k-1} + 1*0$ ===> ${\hat{X}}'_{k} = \hat{X}_{k-1}$ （1）

${P}'_{k} = 1*P_{k-1} *1^{T}+ Q$ ===> ${P}'_{k} = P_{k-1} + Q$ （2）

$G_{k} = {P}'_{k}*1^{T} / (1*{P}'_{k} *1^{T}+ R)$ ===> $G_{k} = {P}'_{k} / ({P}'_{k} + R)$ （3）

$\hat{X}_{k} = {\hat{X}_{k} }' + G_{k}*(Z_{k} - 1*{\hat{X}_{k} }' )$ ===> $\hat{X}_{k} = {\hat{X}_{k} }' + G_{k}*(Z_{k} - {\hat{X}_{k} }' )$ （4）

$P_{k} = (1-G_{k} *1)* {P_{k}}'$ ===> $P_{k} = (1-G_{k} )* {P_{k}}'$ （5）

卡尔曼简化后的公式：

${P}'_{k} = P_{k-1} + Q$ （2）

$G_{k} = {P}'_{k} / ({P}'_{k} + R)$ （3）

$\hat{X}_{k} = \hat{X}_{k-1} + G_{k}*(Z_{k} - \hat{X}_{k-1} )$ （4）

$P_{k} = (1-G_{k} )* {P_{k}}'$ （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()

三者都能得到同一结果