捷联惯导系统学习5.2(递归最小二乘法 )

采用递推方式求解最小二乘法（Recursive Least Square RLS）可以有效减小数据存储量。

测量方程

Z‾k=H‾kX+V‾k\overline Z_k=\overline H_kX+\overline V_kZk=HkX+Vk
Z‾k=[Z1Z2Z3...Zi],H‾k=[H1H2H3...Hi],V‾k=[V1V2V3...Vi]E[V‾i]=0,E[V‾iV‾iT]=R‾i=diag(R1,R2,R3,...)\overline Z_k=\left[\begin{matrix}Z_1\\Z_2\\Z_3\\...\\Z_i \end{matrix}\right],\overline H_k=\left[\begin{matrix}H_1\\H_2\\H_3\\...\\H_i \end{matrix}\right],\overline V_k=\left[\begin{matrix}V_1\\V_2\\V_3\\...\\V_i \end{matrix}\right] \\ E[\overline V_i]=0,E[\overline V_i\overline V_i^T]=\overline R_i=diag(R_1,R_2,R_3,...)Zk=⎣⎢⎢⎢⎢⎡Z1Z2Z3...Zi⎦⎥⎥⎥⎥⎤,Hk=⎣⎢⎢⎢⎢⎡H1H2H3...Hi⎦⎥⎥⎥⎥⎤,Vk=⎣⎢⎢⎢⎢⎡V1V2V3...Vi⎦⎥⎥⎥⎥⎤E[Vi]=0,E[ViViT]=Ri=diag(R1,R2,R3,...)
最小二乘估计
X^WLS=(HTWH)−1HTWZ\hat X_{WLS}=(H^TWH)^{-1}H^TWZX^WLS=(HTWH)−1HTWZ
对测量方程做最小二乘估计
k−1k-1k−1时最小二乘估计
X^k−1=(H‾k−1TR‾k−1−1H‾k−1)−1H‾k−1TR‾k−1−1Z‾k−1=Pk−1H‾k−1TR‾k−1−1Z‾k−1Pk−1=(H‾k−1TR‾k−1−1H‾k−1)−1\hat X_{k-1}=(\overline H_{k-1}^T\overline R^{-1}_{k-1}\overline H_{k-1})^{-1}\overline H_{k-1}^T\overline R^{-1}_{k-1}\overline Z_{k-1}=P_{k-1} \overline H_{k-1}^T\overline R^{-1}_{k-1}\overline Z_{k-1} \\ P_{k-1} =(\overline H_{k-1}^T\overline R^{-1}_{k-1}\overline H_{k-1})^{-1}X^k−1=(Hk−1TRk−1−1Hk−1)−1Hk−1TRk−1−1Zk−1=Pk−1Hk−1TRk−1−1Zk−1Pk−1=(Hk−1TRk−1−1Hk−1)−1
kkk时最小二乘估计
X^k=(H‾kTR‾k−1H‾k)−1H‾kTR‾k−1Z‾k=Pk([H‾k−1THkT][R‾k−1−100Rk−1][Z‾k−1Zk])=Pk(H‾k−1TR‾k−1−1Z‾k−1+HkTRk−1Zk)=X^k−1+PkHkTRk−1(Zk−HkX^k−1)=X^k−1+Kk(Zk−HkX^k−1)\hat X_{k}=(\overline H_{k}^T\overline R^{-1}_{k}\overline H_{k})^{-1}\overline H_{k}^T\overline R^{-1}_{k}\overline Z_{k}\\ =P_k(\left[\begin{matrix} \overline H_{k-1}^T&H_k^T\\ \end{matrix}\right]\left[\begin{matrix} \overline R_{k-1}^{-1}&0\\0&R_k^{-1}\\ \end{matrix}\right]\left[\begin{matrix} \overline Z_{k-1}\\Z_k\\ \end{matrix}\right])\\ =P_k(\overline H_{k-1}^T\overline R^{-1}_{k-1}\overline Z_{k-1} +H_k^TR_k^{-1}Z_k)\\ =\hat X_{k-1}+P_{k}H_{k}^TR_k^{-1}(Z_k-H_k\hat X_{k-1}) \\ =\hat X_{k-1}+K_k(Z_k-H_k\hat X_{k-1})X^k=(HkTRk−1Hk)−1HkTRk−1Zk=Pk([Hk−1THkT][Rk−1−100Rk−1][Zk−1Zk])=Pk(Hk−1TRk−1−1Zk−1+HkTRk−1Zk)=X^k−1+PkHkTRk−1(Zk−HkX^k−1)=X^k−1+Kk(Zk−HkX^k−1)
Pk=H‾kTR‾k−1H‾k=([H‾k−1THkT][R‾k−1−100Rk−1][Z‾k−1Zk])−1=(Pk−1−1+HkTRk−1Hk)−1P_k=\overline H_{k}^T\overline R^{-1}_{k}\overline H_{k}=(\left[\begin{matrix} \overline H_{k-1}^T&H_k^T\\ \end{matrix}\right]\left[\begin{matrix} \overline R_{k-1}^{-1}&0\\0&R_k^{-1}\\ \end{matrix}\right]\left[\begin{matrix} \overline Z_{k-1}\\Z_k\\ \end{matrix}\right])^{-1}=(P_{k-1}^{-1}+H_k^TR_k^{-1}H_k)^{-1}Pk=HkTRk−1Hk=([Hk−1THkT][Rk−1−100Rk−1][Zk−1Zk])−1=(Pk−1−1+HkTRk−1Hk)−1
Kk=PkHkTRk−1K_k=P_{k}H_{k}^TR_k^{-1}Kk=PkHkTRk−1
最小二乘估计递推公式
Pk−1P_{k}^{-1}Pk−1左乘PkP_kPk右乘Pk−1P_{k-1}Pk−1
PkPk−1Pk−1=Pk(Pk−1−1+HkTRk−1Hk)Pk−1P_kP_k^{-1}P_{k-1}=P_k(P_{k-1}^{-1}+H_k^TR_k^{-1}H_k)P_{k-1}PkPk−1Pk−1=Pk(Pk−1−1+HkTRk−1Hk)Pk−1
得到PkP_kPk
Pk=Pk−1−PkHkTRk−1HkPk−1P_k=P_{k-1}^{}-P_kH_k^TR_k^{-1}H_kP_{k-1}Pk=Pk−1−PkHkTRk−1HkPk−1
再将PkP_kPk右乘HkTH_k^THkT整理可得：
Kk=PkHkTRk−1=Pk−1HkT(HkPk−1HkT+Rk)−1K_k=P_kH_k^TR_k^{-1}=P_{k-1}H_k^T(H_kP_{k-1}H_k^T+R_k)^{-1}Kk=PkHkTRk−1=Pk−1HkT(HkPk−1HkT+Rk)−1
得到递推公式为：
{Kk=Pk−1HkT(HkPk−1HkT+Rk)−1Pk=Pk−1−PkHkTRk−1HkPk−1X^k=X^k−1+Kk(Zk−HkX^k−1)\begin{cases} K_k=P_{k-1}H_k^T(H_kP_{k-1}H_k^T+R_k)^{-1}\\ P_k=P_{k-1}^{}-P_kH_k^TR_k^{-1}H_kP_{k-1}\\ \hat X_{k}=\hat X_{k-1}+K_k(Z_k-H_k\hat X_{k-1})\\ \end{cases}⎩⎪⎨⎪⎧Kk=Pk−1HkT(HkPk−1HkT+Rk)−1Pk=Pk−1−PkHkTRk−1HkPk−1X^k=X^k−1+Kk(Zk−HkX^k−1)

捷联惯导系统学习5.2(递归最小二乘法 )相关推荐

捷联惯导系统学习2.5（等效旋转矢量微分方程的泰勒级数解）
在高精度的捷联惯导系统中,陀螺仪姿态的解算往往是通过采集一定时间内的角增量信息, 计算角增量信息计算出等效旋转矢量,在通过等效旋转矢量递推余弦阵或者四元数,完成姿态更新. 等效旋转矢量微分方程的泰勒级 ...
捷联惯导系统学习4.1(惯导数值更新算法)
1 常用坐标系的定义 (1)地心惯性坐标系(i 系,inertial frame) 用oixiyizio_ix_iy_iz_ioixiyizi表示,原点以地球为中心, 原点oio_ioi在地 ...
捷联惯导系统学习7.5(低成本组合导航系统模型)
低成本组合导航系统模型低精度MEMS惯性/卫星/地磁组合导航系统中,选择惯导系统的姿态失准角ϕ\phiϕ.速度误差δvn\delta v^nδvn.定位误差δpn\delta p^nδpn.陀螺仪相 ...
捷联惯导系统学习3.2(地球的正常重力场)
圆球模型下的地球重力如图,重力为引力与离心力作用的共同结果,其中引力:F=GMr2=ur2(G引力常数,M为地球质量,r为质点到地心距离)F=\frac{GM}{r^2}=\frac{u}{r^2 ...
捷联惯导系统学习6.1(一些卡尔曼滤波处理技术 )
噪声相关条件下的Kalman滤波理想状态下的kalman需要系统噪声与测量噪声之间部不相关,如果测量噪声与系统噪声相关需要进行处理噪声相关条件下的系统状态方程 Xk:n维状态向量X_k:n维状态向 ...
捷联惯导系统学习6.6(Sage-Husa自适应滤波 )
原理作用只有准确的获得系统的结构参数和噪声统计特性参数,才能获得最优值的状态估计,实际中往往是不够准确的可以使用量测输出(输出隐含了系统模型的某些信息)对系统系统模型进行重新估计. 量测噪声方差阵 ...
捷联惯导系统学习2.6(圆锥误差补偿多子样算法)
若圆锥运动的四元数更新方程为: Q(tm)=Q(Tm−1).Q(T)Q(t_m)=Q(T_{m-1}).Q(T)Q(tm)=Q(Tm−1).Q(T) ( ...四元数乘法) ( Q(T)Q(T)Q ...
捷联惯导系统学习2.2（等效旋转矢量）
二等效旋转矢量: 1 一些重要的三维矢量运算关系(证明请自己找) $ u为单位矢量 ;u'是u的一阶导数$ (1):V1×(V2×V3)=(V1∗V3)V2−(V1∗V2)V3(1):V_1\tim ...
捷联惯导系统学习2.5（等效旋转矢量微分方程）
已知三维旋转矢量关系如下:(证明略) 参数说明: ViV_iVi表示三维空间矢量 v=∣V∣=VVTv=|V|=\sqrt{VV^T}v=∣V∣=VVT表示矢量模值 uuu为与V同方向的单位矢量即 ...

捷联惯导系统学习5.2(递归最小二乘法 )

测量方程

捷联惯导系统学习5.2(递归最小二乘法 )相关推荐

最新文章

热门文章