无控制项线性动态系统滤波
已知状态方程与观测方程分别为 { X k = ϕ k , k − 1 X k − 1 + B k − 1 U k − 1 + Γ k − 1 W k − 1 Z k = H k X k + Y k + V k \left\{ \begin{aligned} &X_k=\phi_{k, k-1}X_{k-1}+B_{k-1}U_{k-1}+\Gamma_{k-1}W_{k-1}\\ &Z_k=H_kX_k+Y_k+V_k \end{aligned} \right. {Xk=ϕk,k−1Xk−1+Bk−1Uk−1+Γk−1Wk−1Zk=HkXk+Yk+Vk 系统具有性质 { E W k = 0 E W k W i T = Q k δ k i E V k = 0 E V k V i T = R k δ k i c o v ( W k , V i ) = E W k V i T = φ w v ( k ) δ k i \left\{ \begin{aligned} &\mathbb{E}W_k=0\\ &\mathbb{E}W_kW_i^T=Q_k\delta_{ki}\\ &\mathbb{E}V_k=0\\ &\mathbb{E}V_kV_i^T=R_k\delta_{ki}\\ &cov(W_k, V_i)=\mathbb{E}W_kV_i^T=\varphi_{wv}(k)\delta_{ki} \end{aligned} \right. ⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧EWk=0EWkWiT=QkδkiEVk=0EVkViT=Rkδkicov(Wk,Vi)=EWkViT=φwv(k)δki 其中 W k W_k Wk与 V k V_k Vk相关， φ w v ( k ) \varphi_{wv}(k) φwv(k)序列为已知. 由观测方程可知 Z k − 1 − H k − 1 X k − 1 − Y k − 1 − V k − 1 = 0 Z_{k-1}-H_{k-1}X_{k-1}-Y_{k-1}-V_{k-1}=0 Zk−1−Hk−1Xk−1−Yk−1−Vk−1=0 代入系统方程得到 X k = ϕ k , k − 1 + B k − 1 U k − 1 + Γ k − 1 W k − 1 + L k − 1 ( Z k − 1 − H k − 1 X k − 1 − Y k − 1 − V k − 1 ) X_k=\phi_{k, k-1}+B_{k-1}U_{k-1}+\Gamma_{k-1}W_{k-1}+L_{k-1}(Z_{k-1}-H_{k-1}X_{k-1}-Y_{k-1}-V_{k-1}) Xk=ϕk,k−1+Bk−1Uk−1+Γk−1Wk−1+Lk−1(Zk−1−Hk−1Xk−1−Yk−1−Vk−1) 其中 L k − 1 L_{k-1} Lk−1为 n × q n\times q n×q矩阵，为待定系数矩阵，系统方程表示为 X k = ϕ k , k − 1 ∗ X k − 1 + B k − 1 U k − 1 + W k − 1 ∗ + L k − 1 ( Z k − 1 − Y k − 1 ) ⏟ 新的控制项 ϕ k , k − 1 ∗ = ϕ k , k − 1 − L k − 1 H k − 1 W k − 1 ∗ = Γ k − 1 W k − 1 − L k − 1 V k − 1 \begin{aligned} &X_k=\phi_{k, k-1}^*X_{k-1}+\underbrace{B_{k-1}U_{k-1}+W^*_{k-1}+L_{k-1}(Z_{k-1}-Y_{k-1})}_{\text{新的控制项}}\\ &\phi^*_{k, k-1}=\phi_{k, k-1}-L_{k-1}H_{k-1}\\ &W^*_{k-1}=\Gamma_{k-1}W_{k-1}-L_{k-1}V_{k-1} \end{aligned} Xk=ϕk,k−1∗Xk−1+新的控制项 Bk−1Uk−1+Wk−1∗+Lk−1(Zk−1−Yk−1)ϕk,k−1∗=ϕk,k−1−Lk−1Hk−1Wk−1∗=Γk−1Wk−1−Lk−1Vk−1 经过推导（推导过程详细见参考资料）可以得到滤波方程组 { K p ( k ) = Γ k φ w v ( k ) R k − 1 K k = P k ∣ k − 1 H k T [ H k P k ∣ k − 1 H k T + R k ] − 1 P k ∣ k − 1 = [ ϕ k ∣ k − 1 − K p ( k − 1 ) H k − 1 ] P k − 1 [ ϕ k ∣ k − 1 − K p ( k − 1 ) H k − 1 ] T + Γ k − 1 Q k − 1 Γ k − 1 T − K p ( k − 1 ) R k − 1 K p T ( k − 1 ) P k = ( I − K k H k ) P k ∣ k − 1 P 0 = E [ X 0 − E X 0 ] [ X 0 − E X 0 ] T \left\{ \begin{aligned} &K_p(k)=\Gamma_k\varphi_{wv}(k)R_k^{-1}\\ &K_k=P_{k\mid k-1}H_k^T[H_kP_{k\mid k-1}H_k^T+R_k]^{-1}\\ &P_{k\mid k-1}=[\phi_{k\mid k-1}-K_p(k-1)H_{k-1}]P_{k-1}[\phi_{k\mid k-1}-K_p(k-1)H_{k-1}]^T+\\ &\Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^T-K_p(k-1)R_{k-1}K_p^T(k-1)\\ &P_k=(I-K_kH_k)P_{k\mid k-1}\\ &P_0=\mathbb{E}[X_0-\mathbb{E}X_0][X_0-\mathbb{E}X_0]^T \end{aligned} \right. ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧Kp(k)=Γkφwv(k)Rk−1Kk=Pk∣k−1HkT[HkPk∣k−1HkT+Rk]−1Pk∣k−1=[ϕk∣k−1−Kp(k−1)Hk−1]Pk−1[ϕk∣k−1−Kp(k−1)Hk−1]T+Γk−1Qk−1Γk−1T−Kp(k−1)Rk−1KpT(k−1)Pk=(I−KkHk)Pk∣k−1P0=E[X0−EX0][X0−EX0]T
考虑无控制项系统，假设线性系统的动态方程为 X k = ϕ k , k − 1 X K − 1 + Γ k − 1 X_k=\phi_{k, k-1}X_{K-1}+\Gamma_{k-1} Xk=ϕk,k−1XK−1+Γk−1 其中 W k − 1 W_{k-1} Wk−1为有色噪声，噪声系统方程为 W k = F k , k − 1 W k − 1 + μ k − 1 W_k=F_{k, k-1}W_{k-1}+\mu_{k-1} Wk=Fk,k−1Wk−1+μk−1 其中， W k W_k Wk为 S S S维有色噪声， F k , k − 1 F_{k, k-1} Fk,k−1为 S × S S\times S S×S维噪声系统转移矩阵， μ k − 1 \mu_{k-1} μk−1为 S S S维白色噪声， E μ k = 0 , V μ k = Q k \mathbb{E}\mu_k=0, \mathbb{V}\mu_k=Q_k Eμk=0,Vμk=Qk. 组合两个系统得到新的状态变量 T k = [ X k W k ] T_k=\left[ \begin{aligned} X_k\\ W_k \end{aligned} \right] Tk=[XkWk] 令 ϕ k , k − 1 = [ ϕ k , k − 1 Γ k − 1 0 Γ k , k − 1 ] ， G k = [ O s × s I s × s ] \phi_{k, k-1}=\left[ \begin{matrix} &\phi_{k, k-1} & \Gamma_{k-1}\\ &0& \Gamma_{k, k-1} \end{matrix} \right]， G_k=\left[ \begin{matrix} O_{s\times s}\\ I_{s\times s} \end{matrix} \right] ϕk,k−1=[ϕk,k−10Γk−1Γk,k−1]，Gk=[Os×sIs×s] 新系统的状态方程为 Γ k = ϕ k , k − 1 T k − 1 + G k − 1 μ k − 1 \Gamma_k=\phi_{k, k-1}T_{k-1}+G_{k-1}\mu_{k-1} Γk=ϕk,k−1Tk−1+Gk−1μk−1
在Matlab中有专门的Kalman滤波函数，是基于一般性线性定常系统 x ˙ ( k + 1 ) = A x ( k ) + B u ( k ) + G v ( k ) y ( k ) = C x ( k ) + D u ( k ) + H w ( k ) + v ( k ) \dot{x}(k+1)=Ax(k)+Bu(k)+Gv(k)\\ y(k)=Cx(k)+Du(k)+Hw(k)+v(k) x˙(k+1)=Ax(k)+Bu(k)+Gv(k)y(k)=Cx(k)+Du(k)+Hw(k)+v(k) 其中 x ( k ) x(k) x(k)为 k k k时刻的状态向量， u ( k ) u(k) u(k)为 k k k时刻的控制向量， w ( k ) w(k) w(k)为状态随机噪声， v ( k ) v(k) v(k)为观测随机噪声. 编程步骤如下： 1.首先给动态方程中各矩阵 A , B , C , D , G , H A, B, C, D, G, H A,B,C,D,G,H赋值 2.给定 A , B , C , D , G , H A, B, C, D, G, H A,B,C,D,G,H后描述系统SYS=SS(A, [B, G], C, [D H], T)，其中 T T T表示离散系统的采样时间间隔，-1表示使用默认值. 3.调用Kalman滤波函数 [KEST, L, P, M, Z]=kalman(SYS, QN, RN, NN)，其中SYS表示系统，QN表示状态噪声的方差阵，RN表示观测噪声的方差阵，NN表示状态噪声和观测噪声的协方差阵；在输出结果中，KEST表示系统输出估计值 y ^ \hat{y} y^和状态估计值 x ^ \hat{x} x^，L和M表示两个增益矩阵，P和Z分别表示误差方差阵和滤波误差方差阵. 4.根据公式 x ^ ( k + 1 ∣ k ) = A x ^ ( k ∣ k − 1 ) + L ( y ( k ) − C x ^ ( k ∣ k − 1 ) ) x ^ ( k ∣ k ) = x ^ ( k ∣ k − 1 ) + M ( y ( k ) − C x ^ ( k ∣ k − 1 ) ) \begin{aligned} &\hat{x}(k+1\mid k)=A\hat{x}(k\mid k-1)+L(y(k)-C\hat{x}(k\mid k-1))\\ &\hat{x}(k\mid k)=\hat{x}(k\mid k-1)+M(y(k)-C\hat{x}(k\mid k-1)) \end{aligned} x^(k+1∣k)=Ax^(k∣k−1)+L(y(k)−Cx^(k∣k−1))x^(k∣k)=x^(k∣k−1)+M(y(k)−Cx^(k∣k−1)) 计算 x ( k ∣ k ) x(k\mid k) x(k∣k). 例题：给定矩阵 A = [ 0.5 0.3 0.4 0.5 − 0.4 0.4 − 0.1 0.4 0.3 ] ， C = [ 0 1 0 ] A=\left[ \begin{matrix} 0.5 & 0.3 & 0.4\\ 0.5 & -0.4 & 0.4\\ -0.1 & 0.4 & 0.3 \end{matrix} \right]， C=\left[ \begin{matrix} 0 & 1 & 0 \end{matrix} \right] A=⎣⎡0.50.5−0.10.3−0.40.40.40.40.3⎦⎤，C=[010] 状态初值 x ˉ 1 = [ 0.85 1.25 0.39 ] \bar{x}_1=\left[ \begin{matrix} 0.85 \\ 1.25\\ 0.39 \end{matrix} \right] xˉ1=⎣⎡0.851.250.39⎦⎤ (1)构造一个模拟动态系统 x k + 1 = A x k + e k ， y k = [ 0 1 0 ] x k ， x ˉ 1 = [ 0.85 1.25 0.39 ] x_{k+1}=Ax_k+e_k，y_k=[0\quad 1 \quad 0]x_k，\bar{x}_1=\left[ \begin{matrix} 0.85\\ 1.25\\ 0.39 \end{matrix} \right] xk+1=Axk+ek，yk=[010]xk，xˉ1=⎣⎡0.851.250.39⎦⎤ 其中 { e i } \{e_i\} {ei}和 { v i } \{v_i\} {vi}是互不相关的白噪声，计算系统的模拟输出 (2).调用Kalman滤波函数，得到输出值 y 1 , y 2 , … , y 20 y_1, y_2, \dots, y_{20} y1,y2,…,y20进行滤波得到模拟系统状态滤波值 x ^ 1 , x ^ 2 , … , x ^ 20 \hat{x}_1, \hat{x}_2,\dots, \hat{x}_{20} x^1,x^2,…,x^20，对比 x ˉ k ( i ) \bar{x}_k(i) xˉk(i)和 x ^ k ( i ) , k = 1 , 2 , … , 20 ; i = 1 , 2 , 3 \hat{x}_k(i),k=1, 2,\dots, 20; i=1, 2, 3 x^k(i),k=1,2,…,20;i=1,2,3. 解析： (1). 构造模拟系统，根据递推公式 x k + 1 = A x k x_{k+1}=Ax_k xk+1=Axk和状态初值 x ˉ 1 \bar{x}_1 xˉ1，得到确定值 x ˉ 2 , x ˉ 3 , … , x ˉ 20 \bar{x}_2, \bar{x}_3, \dots, \bar{x}_{20} xˉ2,xˉ3,…,xˉ20. (2).模拟生成标准正态随机噪声 e 1 , e 2 , … , e 20 e_1,e_2, \dots, e_{20} e1,e2,…,e20并分别于 x ˉ 1 , x ˉ 2 , … , x ˉ 20 \bar{x}_1, \bar{x}_2, \dots, \bar{x}_{20} xˉ1,xˉ2,…,xˉ20叠加得到 x i = x ˉ i + e i x_i=\bar{x}_i+e_i xi=xˉi+ei (3).令 y ˉ k = C x k = [ 0 1 0 ] [ x k ( 1 ) x k ( 2 ) x k ( 3 ) ] = x k ( 2 ) \bar{y}_k=Cx_k=[0\quad 1\quad 0]\left[ \begin{matrix}x_k(1)\\x_k(2)\\x_k(3)\end{matrix}\right]=x_k(2) yˉk=Cxk=[010]⎣⎡xk(1)xk(2)xk(3)⎦⎤=xk(2)，生成标准随机噪声 v 1 , v 2 , … , v 20 , E [ v ( i ) e ( j ) ] = 0 v_1, v_2, \dots, v_{20}, \mathbb{E}[v(i)e(j)]=0 v1,v2,…,v20,E[v(i)e(j)]=0，并分别于 x ˉ 1 , x ˉ 2 , … , x ˉ 20 \bar{x}_1, \bar{x}_2, \dots, \bar{x}_{20} xˉ1,xˉ2,…,xˉ20叠加得到模拟输出 y i = y ˉ i + v i , i = 1 , 2 , … , 20 y_i=\bar{y}_i+v_i, i=1, 2, \dots, 20 yi=yˉi+vi,i=1,2,…,20 matlab计算代码
clc; clear all; close all; %-----Q1------------------ % init values x(:, 1)=[8.539; 12.481; -3.924]; % init state x_e(:, 1)=[15; 16; 17]; A=[ 0.5, 0.3, 0.4; 0.5, -0.4, 0.4; -0.1, 0.4, 0.3 ]; B=zeros(3, 3); G=eye(3, 3); C=[0, 1, 0]; D=[0, 0, 0]; H=zeros(1, 3); for i = 2:20 % model noise ~ N(0, 1) w=randn(3, 1); % obs noise ~ N(0, 1) v=randn(3, 1); x(:, i)=A*x(:, i-1); % add model noise x1(:, i)=x(:, i)+w; QN=eye(2, 2); RN=1; NN=0; % add obs noise z0(:, i)=C*x1(:, i)+v; %-----Q2---------------- SYS=ss(A, [B G], C, [D H], -1); [KEST, L, P]=kalman(SYS, QN, RN, NN); % get filteration x_e(:, i)=A*x_e(:, i-1)+L.*(z0(:, i)-C*A*x_e(:, i-1)); end % plot and compare t=1:20; subplot(2, 2, 1), plot(t, x(1, :), 'b', t, x_e(1, :), 'r'); subplot(2, 2, 2), plot(t, x(2, :), 'b', t, x_e(2, :), 'r'); subplot(2, 2, 3), plot(t, x(3, :), 'b', t, x_e(3, :), 'r');得到滤波图像如下 可以发现随着采样时间间隔的增加，滤波后的信号和真实信号逐渐趋同.
现代控制理论（第二版） 清华大学出版社 张嗣瀛 高立群 卡尔曼滤波算法推导
