RLS算法到卡尔曼滤波 I

我的这篇文章写了RLS算法的直接动机。
https://blog.csdn.net/ZLH_HHHH/article/details/89061839
数学上等价于最小二乘法。
接着《 R L S RLS RLS算法》这篇文章来说。

P P P矩阵到意义

回想,最初目标函数的定义:
J = E ( ( Y ( k ) − H ( k ) x ^ ( k ) ) T ( Y ( k ) − H ( k ) x ^ ( k ) ) ) = E ( ( H ( k ) x + v ( k ) − H ( k ) x ^ ( k ) ) T ( H ( k ) x + v ( k ) − H ( k ) x ^ ( k ) ) ) J=E\Big((Y(k)-H(k)\hat x(k))^T(Y(k)-H(k)\hat x(k))\Big)\\ =E\Big((H(k)x+v(k)-H(k)\hat x(k))^T(H(k)x+v(k)-H(k)\hat x(k))\Big) J=E((Y(k)H(k)x^(k))T(Y(k)H(k)x^(k)))=E((H(k)x+v(k)H(k)x^(k))T(H(k)x+v(k)H(k)x^(k)))
解释一下,这里 v ( k ) v(k) v(k)是测量误差,是不可观测的。区别于估计误差。一般假设其均值为 0 0 0,即: E ( v ) = 0 E(v)=0 E(v)=0,即无偏。
那么:
J = E [ ( x ^ ( k ) − x ) T H T ( k ) H ( k ) ( x ^ ( k ) − x ) ] J=E\Big[\Big(\hat x(k)-x\Big)^TH^T(k)H(k)\Big(\hat x(k)-x\Big)\Big] J=E[(x^(k)x)THT(k)H(k)(x^(k)x)]
这也就是说,最小化 J J J时,等价于最小话:
J ′ = E ( ( x ^ ( k ) − x ) T ( x ^ ( k ) − x ) ) J'=E\Big((\hat x(k)-x)^T(\hat x(k)-x)\Big) J=E((x^(k)x)T(x^(k)x))
最小化上式,意味着新的一次估计更加接近真实的 x x x
令: A ( k ) = E ( ( x ^ ( k ) − x ) ( x ^ ( k ) − x ) T ) A(k)=E\Big((\hat x(k)-x)(\hat x(k)-x)^T\Big) A(k)=E((x^(k)x)(x^(k)x)T) 则有: J ′ = T r ( A ( k ) ) J'=Tr(A(k)) J=Tr(A(k))
最小化 J ′ J' J与最小化 J J J最后结果是一样的。
考虑计算最佳增益 K K K
x ^ ( k ) − x = x ^ ( k − 1 ) + K ( k ) ( y ( k ) − h ( k ) x ^ ( k − 1 ) ) − x = x ^ ( k − 1 ) + K ( k ) ( h ( k ) x + v ( k ) − h ( k ) x ^ ( k − 1 ) ) − x = ( I − K ( k ) h ( k ) ) ( x ^ ( k − 1 ) − x ) + K ( k ) v ( k ) \hat x(k)-x=\hat x(k-1)+K(k)\Big(y(k)-h(k)\hat x(k-1)\Big)-x\\ =\hat x(k-1)+K(k)\Big(h(k)x+v(k)-h(k)\hat x(k-1)\Big)-x \\ = (I-K(k)h(k))(\hat x(k-1)-x)+K(k)v(k) x^(k)x=x^(k1)+K(k)(y(k)h(k)x^(k1))x=x^(k1)+K(k)(h(k)x+v(k)h(k)x^(k1))x=(IK(k)h(k))(x^(k1)x)+K(k)v(k)

那么:
A ( k ) = ( I − K ( k ) h ( k ) ) A ( k − 1 ) ( I − K ( k ) h ( k ) ) T + K ( k ) R ( k ) K T ( k ) A(k)=(I-K(k)h(k))A(k-1)(I-K(k)h(k))^T+K(k)R(k)K^T(k) A(k)=(IK(k)h(k))A(k1)(IK(k)h(k))T+K(k)R(k)KT(k)
在这里, R ( k ) = E ( v 2 ( k ) ) R(k)=E(v^2(k)) R(k)=E(v2(k)).
计算最优增益:
∂ T r ( A ( k ) ) ∂ K ( k ) = − 2 h ( k ) A ( k − 1 ) + 2 h ( k ) A ( k − 1 ) h T ( k ) K T ( k ) + 2 R ( k ) K T ( k ) = 0 \frac{\partial Tr(A(k))}{\partial K(k)}=-2h(k)A(k-1)+2h(k)A(k-1)h^T(k)K^T(k)+2R(k)K^T(k)=0 K(k)Tr(A(k))=2h(k)A(k1)+2h(k)A(k1)hT(k)KT(k)+2R(k)KT(k)=0
h ( k ) A ( k − 1 ) + ( ( h ( k ) A ( k − 1 ) h T ( k ) + R ( k ) ) K T ( k ) = 0 h(k)A(k-1)+\Big((h(k)A(k-1)h^T(k)+R(k)\Big)K^T(k)=0 h(k)A(k1)+((h(k)A(k1)hT(k)+R(k))KT(k)=0
所以: K ( k ) = A ( k − 1 ) h T ( k ) R ( k ) + h ( k ) A ( k − 1 ) h T ( k ) K(k)=\frac{A(k-1)h^T(k)}{R(k)+h(k)A(k-1)h^T(k)} K(k)=R(k)+h(k)A(k1)hT(k)A(k1)hT(k)
显然,上述更新方式和带权最小二乘法是一样的。
因为:
K ( k ) = P ( k ) h T ( k ) R − ( k ) K(k)=P(k)h^T(k)R^-(k) K(k)=P(k)hT(k)R(k)
A A A递推式子,替换 P P P
( I − K ( k ) h ( k ) ) P ( k − 1 ) ( I − K ( k ) h ( k ) ) T + K ( k ) R ( k ) K T ( k ) = P ( k ) − P ( k ) h T ( k ) K T ( k ) + P ( k ) h T ( k ) R − ( k ) R ( k ) K T ( k ) = P ( k ) (I-K(k)h(k))P(k-1)(I-K(k)h(k))^T+K(k)R(k)K^T(k)\\= P(k)-P(k)h^T(k)K^T(k)+P(k)h^T(k)R^-(k)R(k)K^T(k)\\ =P(k) (IK(k)h(k))P(k1)(IK(k)h(k))T+K(k)R(k)KT(k)=P(k)P(k)hT(k)KT(k)+P(k)hT(k)R(k)R(k)KT(k)=P(k)
回想 J ′ J' J最小化时, J J J最小化,有足够的理由,认为 P ( k ) P(k) P(k)就是 x ^ ( k ) \hat x(k) x^(k)的协方差矩阵,即为:
P ( k ) = ( H T ( k ) H ( k ) ) − = E ( ( x ^ ( k ) − x ) ( x ^ ( k ) − x ) T ) P(k)=\Big(H^T(k)H(k)\Big)^-=E\Big((\hat x(k)-x)(\hat x(k)-x)^T\Big) P(k)=(HT(k)H(k))=E((x^(k)x)(x^(k)x)T)

用于滤波,rls自适应滤波 #include #include #include #define N 110000 float x[N]; float x1[110050]; float xout[N]; float w[50]; float p[50][50]; float u[50]; float kn[50]; float kn1[50]; float dn; float e; float outmiddle; //阶数50// //****************************************初始化程序用于计算w[][].p[]**********************************// float initial () { char i1,j1; for(i1=0;i1<50;i1++) { w[i1]=0; } for(i1=0;i1<50;i1++) { for(j1=0;j1<50;j1++) { p[i1][j1]=0; } } for(i1=0;i1<50;i1++) { p[i1][i1]=9999999.9; } return(0); } //****************************************************************************************************************// //****************************************e[n]的求解**************************************************************// float en(unsigned long int datanumber) { /*float result=0; int i2,k2; int j2; j2=datanumber; k2=datanumber+5; e=0; for(i2=0;i2<50;i2++)//计算w()*u() { e=e+w[i2]*u[i2]; } dn=0; for(i2=j2;i2<k2;i2++) { dn=dn+x[i2]; } dn=dn/5;//给定dn为0.2 e=dn-e; return(0);*/ } //****************************************************************************************************************// //****************************************k[n]的求解**************************************************************// void knkn() { int i3,j3; float knmiddle=0; float middle[50]; float pluse=0; for(i3=0;i3<50;i3++) { for(j3=0;j3<50;j3++) { knmiddle=knmiddle+p[i3][j3]*u[j3]; } kn[i3]=knmiddle; knmiddle=0; }//计算p(n-1)*u(n) for(i3=0;i3<50;i3++) { middle[i3]=kn[i3]; } for(i3=0;i3<50;i3++) { pluse=pluse+middle[i3]*u[i3]; }//计算uh(n)*p(n-1)*u(n) pluse=pluse+0.7;//衰减因数0.7 for(i3=0;i3<50;i3++) { kn[i3]=kn[i3]/pluse; } pluse=0; } //****************************************************************************************************************// //*******************************************用于pn[]的更新*******************************************************// float newpn() { float newpn[50][50]; float newpn1[50][50]; float newpn3[50][50]; float newpn2=0; int i4,j4,k4; for(i4=0;i4<50;i4++) { for(j4=0;j4<50;j4++) { newpn[i4][j4]=kn[i4]*u[j4]; newpn3[i4][j4]=p[i4][j4]; } }//计算k(n)*u(n) for(i4=0;i4<50;i4++) { for(j4=0;j4<50;j4++) { for(k4=0;k4<50;k4++) { newpn2=newpn2+newpn[i4][k4]*newpn3[k4][j4]; } newpn1[i4][j4]=newpn2; newpn2=0; } }//计算k()*u()*p() for(i4=0;i4<50;i4++) { for(j4=0;j4<50;j4++) { p[i4][j4]=p[i4][j4]-newpn1[i4][j4]; p[i4][j4]=p[i4][j4]/0.7;//衰减因子是0.7 } } return(0); } //*****************************************************************************************************************// //*************************************用于wn[]的更新**************************************************************// float newwn() { char i5,j5; for(i5=0;i5<50;i5++) { kn1[i5]=kn[i5]; } for(i5=0;i5<50;i5++) { kn1[i5]=kn1[i5]*e; w[i5]=w[i5]+kn1[i5]; } return(0); } main() { //, f[M][N], g[M][N], a[M][M], P[N], K[M]; //float x1[200]; //float Knum = 0, Kden = 0, sum = 0, eP; // int i, j, k, m, n; float dn; long int i=0, j=0,k=0,m=0,n1,n2,n3; float result=0; int i2,k2; int j2; char controller=1; FILE * fpr, * fp_out; int max[2][2]={1,2,3,4}; int max1[2]={1,2}; int max2[2]={2,3}; int maxmiddle; //a[0][1] = 1; if((fpr = fopen("wavewavedata.bin","rb")) == NULL) {printf("cannot open file\n"); exit(0); } for(i=0;i<N;i++) { fread(&x[i], 4, 1, fpr); //printf("%f\n",x[i]); } /*for(i=0;i<49;i++) { x1[i]=x[110000-i-1]; } for(i=49;i<110049;i++) { x1[i]=x[i-49]; }//得到要处理的110049个数据*/ fclose(fpr); for(i=0;i<N;i++) { xout[i]=x[i]; } if((fp_out = fopen("bbboutdata.bin","w")) == NULL) { printf("cannot open file\n"); return; } else printf("success fp_out = %d\n",fp_out); for(j=0; j<N; j++) { fwrite(&xout[j],4, 1, fp_out); //fread(&x1[i][j],4, 1, fpr); //printf("%f\n",xout[j]); } fclose(fp_out); getchar(); initial (); for(m=0;m<N;m++) { //m=0; n1=m; n2=m+50; k=49; for(i=n1;i<n2;i++) //取出要处理的50个数据放于u[n] { u[k]=x1[i]; k--; } //printf("%f\n",u[0]); while(controller) { //en(n3); j2=n2-3; k2=n2+2; e=0; for(i2=0;i2<50;i2++)//计算w()*u() { e=e+w[i2]*u[i2]; } dn=0; for(i2=j2;i2<k2;i2++) { dn=dn+x1[i2]; } dn=dn/5;//给定dn为0.2 e=dn-e; if(e<0.000000001) { controller=0; /*printf("success!",m); for(i=0;i<50;i++) { printf("%f\n",w[i]); } printf("success!",m); for(i=0;i<50;i++) { printf("%f\n",kn[i]); }*/ } else { knkn(); newpn(); newwn(); } } /*for(j=0;j<50;j++) { printf("%f\n",w[j]); }*/ outmiddle=0; for(i=0;i<50;i++) { outmiddle=outmiddle+w[i]*u[i]; } xout[m]=outmiddle; //printf("output",m); //printf("%f\n",xout[m]); } /*for(i=0;i<N;i++) { //xout[i]=x[i]; printf("%f\n",xout[i]); }*/ /*if((fp_out = fopen("bboutdata.bin","w")) == NULL) { printf("cannot open file\n"); return; } else printf("success fp_out = %d\n",fp_out); for(j=0; j<N; j++) { fwrite(&xout[j],4, 1, fp_out); //fread(&x1[i][j],4, 1, fpr); //printf("%f\n",xout[j]); } fclose(fp_out); getchar();*/ }
自适应卡尔曼滤波算法(Adaptive Kalman Filter,AKF)是一种在估计系统状态时能够适应系统动态变化的滤波算法卡尔曼滤波算法是一种基于贝叶斯滤波理论的优化算法,用于估计线性系统的状态。它通过结合系统的观测和模型的预测来最优地估计系统的状态。 然而,传统的卡尔曼滤波算法假设系统的模型参数和观测噪声的统计特性是恒定不变的。在实际应用中,系统的模型参数和观测噪声往往是随时间动态变化的。这种动态变化可能导致传统卡尔曼滤波算法的估计结果不准确。 为了解决这个问题,自适应卡尔曼滤波算法引入了自适应因子和自适应测量噪声协方差矩阵。自适应因子用于调整卡尔曼增益,以适应系统模型参数的变化;自适应测量噪声协方差矩阵用于反映观测噪声的统计特性的变化。 具体实现上,自适应卡尔曼滤波算法使用递归最小二乘法(Recursive Least Squares,RLS)方法来估计系统模型参数和观测噪声的统计特性。通过递归地更新这些参数和特性,自适应卡尔曼滤波算法能够在保持较高准确性的同时适应系统的动态变化。 总之,自适应卡尔曼滤波算法是一种能够自适应估计系统状态的滤波算法,通过引入自适应因子和自适应测量噪声协方差矩阵,能够在系统模型参数和观测噪声统计特性动态变化的情况下保持较高的估计准确性。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值