带噪声估计的KF
x ^ ( k ∣ k − 1 ) = A x ^ q ( k − 1 ∣ k − 1 ) x ^ q ( k ∣ k − 1 ) = x ^ ( k ∣ k − 1 ) + q ^ ( k − 1 ) (92、94) \begin{aligned} \hat{x}(k|k-1)&=A\hat{x}_{q}(k-1|k-1) \\ \hat{x}_{q}(k|k-1)&=\hat{x}(k|k-1)+\hat{q}(k-1) \end{aligned} \tag{92、94} x^(k∣k−1)x^q(k∣k−1)=Ax^q(k−1∣k−1)=x^(k∣k−1)+q^(k−1)(92、94)
P ( k ∣ k − 1 ) = A P Q ( k − 1 ∣ k − 1 ) A ⊤ P Q ( k ∣ k − 1 ) = P ( k ∣ k − 1 ) + Q ^ ( k − 1 ) (95、96) \begin{aligned} P(k|k-1)&=AP_{Q}(k-1|k-1)A^{\top} \\ P_{Q}(k|k-1)&=P(k|k-1)+\hat{Q}(k-1) \end{aligned} \tag{95、96} P(k∣k−1)PQ(k∣k−1)=APQ(k−1∣k−1)A⊤=P(k∣k−1)+Q^(k−1)(95、96)
z ^ ( k ∣ k − 1 ) = H x ^ q ( k ∣ k − 1 ) z ^ r ( k ∣ k − 1 ) = z ^ ( k ∣ k − 1 ) + r ^ ( k − 1 ) (97、99) \begin{aligned} \hat{z}(k|k-1)&=H\hat{x}_{q}(k|k-1) \\ \hat{z}_{r}(k|k-1)&=\hat{z}(k|k-1)+\hat{r}(k-1) \end{aligned} \tag{97、99} z^(k∣k−1)z^r(k∣k−1)=Hx^q(k∣k−1)=z^(k∣k−1)+r^(k−1)(97、99)
P z z = H P Q ( k ∣ k − 1 ) H ⊤ P z z R = P z z + R ^ ( k − 1 ) P x z = P Q ( k ∣ k − 1 ) H ⊤ (100、101、102) \begin{aligned} P_{zz}&=HP_{Q}(k|k-1)H^{\top} \\ {P_{zz}}_{R}&=P_{zz}+\hat{R}(k-1) \\ P_{xz}&=P_{Q}(k|k-1)H^{\top} \\ \end{aligned} \tag{100、101、102} PzzPzzRPxz=HPQ(k∣k−1)H⊤=Pzz+R^(k−1)=PQ(k∣k−1)H⊤(100、101、102)
z ~ ( k ) = z ( k ) − z ^ r ( k ∣ k − 1 ) K ( k ) = P x z P z z R − 1 (103、108) \begin{aligned} \tilde{z}(k)&=z(k)-\hat{z}_{r}(k|k-1) \\ K(k)&=P_{xz}{P_{zz}}_{R}^{-1} \\ \end{aligned} \tag{103、108} z~(k)K(k)=z(k)−z^r(k∣k−1)=PxzPzzR−1(103、108)
P Q ( k ∣ k ) = [ ( I − K ( k ) H ] P Q ( k ∣ k − 1 ) x ^ q ( k ∣ k ) = x ^ q ( k ∣ k − 1 ) + K ( k ) z ~ ( k ) (109、104) \begin{aligned} P_{Q}(k|k)&=[(I-K(k)H]P_{Q}(k|k-1) \\ \hat{x}_{q}(k|k)&=\hat{x}_{q}(k|k-1)+K(k)\tilde{z}(k) \end{aligned} \tag{109、104} PQ(k∣k)x^q(k∣k)=[(I−K(k)H]PQ(k∣k−1)=x^q(k∣k−1)+K(k)z~(k)(109、104)
q ^ ( k ) = [ 1 − d ( k ) ] q ^ ( k − 1 ) + d ( k ) [ x ^ q ( k ∣ k ) − x ^ ( k ∣ k − 1 ) ] r ^ ( k ) = [ 1 − d ( k ) ] r ^ ( k − 1 ) + d ( k ) [ z ( k ) − z ^ ( k ∣ k − 1 ) ] Q ^ ( k ) = [ 1 − d ( k ) ] Q ^ ( k − 1 ) + d ( k ) [ K ( k ) z ~ ( k ) z ~ ( k ) ⊤ K ( k ) ⊤ + P Q ( k ∣ k ) − P ( k ∣ k − 1 ) ] R ^ ( k ) = [ 1 − d ( k ) ] R ^ ( k − 1 ) + d ( k ) [ z ~ ( k ) z ~ ( k ) ⊤ − P z z ] (105、106、110、107) \begin{aligned} \hat{q}(k)&=[1-d(k)]\hat{q}(k-1)+d(k)[\hat{x}_{q}(k|k)-\hat{x}(k|k-1)] \\ \hat{r}(k)&=[1-d(k)]\hat{r}(k-1)+d(k)[z(k)-\hat{z}(k|k-1)] \\ \hat{Q}(k)&=[1-d(k)]\hat{Q}(k-1)+d(k)[K(k)\tilde{z}(k)\tilde{z}(k)^{\top}K(k)^{\top}+P_{Q}(k|k)-P(k|k-1)] \\ \hat{R}(k)&=[1-d(k)]\hat{R}(k-1)+d(k)[\tilde{z}(k)\tilde{z}(k)^{\top}-P_{zz}] \end{aligned} \tag{105、106、110、107} q^(k)r^(k)Q^(k)R^(k)=[1−d(k)]q^(k−1)+d(k)[x^q(k∣k)−x^(k∣k−1)]=[1−d(k)]r^(k−1)+d(k)[z(k)−z^(k∣k−1)]=[1−d(k)]Q^(k−1)+d(k)[K(k)z~(k)z~(k)⊤K(k)⊤+PQ(k∣k)−P(k∣k−1)]=[1−d(k)]R^(k−1)+d(k)[z~(k)z~(k)⊤−Pzz](105、106、110、107)
