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)

  • 2
    点赞
  • 6
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
用于滤波,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();*/ }

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值