卡尔曼滤波原理

假设有一个离散系统
$$\begin{array}{rlrl}x(k)& =Ax(k-1)+B(u(k)+w(k))& & w(k)\sim N(0,Q)\\ y(k)& =Cx(k)+v(k)& & v(k)\sim N(0,R)\end{array}$$
其中$w$是系统噪声，$v$是观测噪声，假设所有的状态和噪声都服从高斯分布。

现在假设知道了 k-1 时刻的后验状态估计$\hat{x}(k-1)$和协方差$\hat{P}(k-1)$，要根据 k 时刻的输入和输出，确定$x(k)$的后验分布。假设$\hat{x}(k)$表示后验，$\check{x}(k)$表示先验。

卡尔曼滤波的第一步（预测），通过状态方程确定$x(k)$的先验分布，状态方程是线性的，高斯分布经过线性变换后仍是高斯分布。因此有：
$$P(x(k)|x(0),u(1:k),y(1:k-1))\sim N(A\hat{x}(k-1)+Bu(k),A\hat{P}(k-1)A^T+BQ{B}^T)$$
这一步显示了如何从上一时刻状态和有噪声输入信息推断出当前时刻的状态分布，这个分布就是先验。记
$$\check{x}(k)=A\hat{x}(k-1)+Bu(k)\check{P}(k)=A\hat{P}(k-1)A^T+BQ{B}^T$$
即：
$$P(x(k)|x(0),u(1:k),y(1:k-1))\sim N(\check{x}(k),\check{P}(k))$$
显然这个状态有很大的不确定性，因为系统有噪声，另外，可以计算该状态下的输出：
$$P(y(k)|x(k))\sim N(Cx(k),R)$$
根据贝叶斯公式，可以计算当前时刻的状态分布的后验概率，即
$$\begin{array}{rl}\underset{\text{后验 }}{\underbrace{P\left(x(k)\mid x(0),u(1:k),y(1:k)\right)}}& =\frac{P\left(x(k),x(0),u(1:k),y(1:k)\right)}{P\left(x(0),u(1:k),y(1:k)\right)}=\frac{P\left(y(k)\mid x(k)\right)P\left(x(k)\mid x(0),u(1:k),y(1:k-1)\right)}{P\left(x(0),u(1:k),y(1:k)\right)}\\ & \propto \underset{\text{似然 }}{\underbrace{P\left(y(k)\mid x(k)\right)}}\underset{\text{先验}}{\underbrace{P\left(x(k)\mid x(0),u(1:k),y(1:k-1)\right)}}\end{array}$$
最后会得到一个关于$x(k)$的高斯分布，将结果设为$x(k)\sim N(\hat{x}(k),\hat{P}(k))$，那么：
$$N(\hat{x}(k),\hat{P}(k))=\eta \cdot N(Cx(k),R)\cdot N(\check{x}(k),\check{P}(k))$$
用取巧的方法，我们已经知道两边都是高斯分布，只需比较$x(k)$的系数，忽略高斯分布前面的$\eta$因子，将等式部分展开：有
$${\left(x(k)-\hat{x}(k)\right)}^T{\hat{P}}_k^{-1}\left(x(k)-\hat{x}(k)\right)={\left(y(k)-Cx(k)\right)}^T{R}^{-1}\left(y(k)-Cx(k)\right)+{\left(x(k)-\check{x}(k)\right)}^T{\check{P}}_k^{-1}\left(x(k)-\check{x}(k)\right)$$
这里的等号并不严格，允许与$x(k)$无关的常数，我们需要求出左侧的$\hat{x}(k),{\hat{P}}_k$，将两边展开，分别比较$x(k)$的二次和一次项系数。

对于二次项系数

有：
$${\hat{P}}_k^{-1}=C^T{R}^{-1}C+{\check{P}}_k^{-1}$$
定义中间变量：
$$K={\hat{P}}_k{C}^T{R}^{-1}$$
将${\hat{P}}_k^{-1}=C^T{R}^{-1}C+{\check{P}}_k^{-1}$两边左乘${\hat{P}}_k$，有
$$I={\hat{P}}_k{C}^T{R}^{-1}C+{\hat{P}}_k{\check{P}}_k^{-1}=KC+{\hat{P}}_k{\check{P}}_k^{-1}$$
于是有：
$${\hat{P}}_k=(I-KC){\check{P}}_k$$
这里有点循环定义的意思，由${\hat{P}}_k$定义了$K$，又把${\hat{P}}_k$写成了$K$的表达式。实际中$K$可以不依靠${\hat{P}}_k$算得（需要用到SMW公式）：
$$K={\check{P}}_k{C}^T(C{\check{P}}_k{C}^T+R{)}^{-1}$$

对于一次项系数

有：
$$-2\hat{x}(k{)}^T{\hat{P}}_k^{-1}=-2y(k{)}^T{R}^{-1}C-2\check{x}(k{)}^T{\check{P}}_k^{-1}$$
取系数转置得：
$${\hat{P}}_k^{-1}\hat{x}(k)=C^T{R}^{-1}y(k)+{\check{P}}_k^{-1}\check{x}(k)$$
两侧左乘${\hat{P}}_k$，得：
$$\hat{x}(k)={\hat{P}}_k{C}^T{R}^{-1}y(k)+{\hat{P}}_k{\check{P}}_k^{-1}\check{x}(k)$$
$$\begin{array}{rl}\hat{x}(k)& ={\hat{P}}_k{C}^T{R}^{-1}y(k)+{\hat{P}}_k{\check{P}}_k^{-1}\check{x}(k)\\ & =Ky(k)+(I-KC)\check{x}(k)\\ & =\check{x}(k)+K(y(k)-C\check{x}(k))\end{array}$$
由此得到了后验均值的表示，上面两个步骤可以归纳为“预测”和“更新”。流程如下所示：

预测：
$$\check{x}(k)=A\hat{x}(k-1)+B{u}_k{\check{P}}_k=A{\hat{P}}_{k-1}{A}^T+BQ{B}^T$$

更新：先计算K，它又称卡尔曼增益：
$$K={\check{P}}_k{C}^T(C{\check{P}}_k{C}^T+R{)}^{-1}$$
然后计算后验概率分布：
$$\begin{array}{rl}{\hat{P}}_k& =(I-KC){\check{P}}_k\\ \hat{x}(k)& =\check{x}(k)+K(y(k)-C\check{x}(k))\\ \hat{y}(k)& =C\hat{x}(k)\end{array}$$

仿真

假设控制对象为：
$$P(s)=\frac{133}{s^2+25s}$$
仿真代码如下

% created by hyacinth on 2024/1/5
clc
clear
close all

%%
fs = 1e3;
dt = 1/fs;

a = 25; b = 133;
sys = tf(b,[1 a 0]);
dsys = c2d(sys,dt,'zoh');
[num, den] = tfdata(dsys,'v');

A1 = [0 1; 0 -a];
B1 = [0; b];
C1 = [1, 0];
D1 = 0;
% [A1, B1 ,C1 ,D1] = tf2ss(133,[1 25 0]);
[A, B, C, D] = c2dm(A1, B1, C1, D1, dt, "zoh");

u_1 = 0; u_2 = 0;
y_1 = 0; y_2 = 0;
ei = 0; e_1 = 0;

N = 1000;
t = (1:N)*dt;
yd = ones(1,N);
w = 0.002*randn(1,N); % system noise
v = 0.002*randn(1,N); % observational noise

Q = 1; R = 1;
P = B*Q*B';
x = zeros(2, 1);

S = 2; % flag
kp = 8; ki = 0.8; kd = 0.2;
threshold = 30;

for n=1:1:N
    y(n) = -den(2)*y_1 - den(3)*y_2 + num(2)*u_1 + num(3)*u_2;
    yv(n) = y(n) + v(n); % add observational noise

    if S==1 % no kalman filter
        y(n) = yv(n);
    elseif S==2 % using kalman filter
        P = A*P*A' + B*Q*B';

        K = P*C'/(C*P*C'+R);
        P = (eye(2) - K*C)*P;

        x = x + K*(yv(n) - C*x);
        y(n) = C*x;
    end

    e(n) = yd(n) - y(n); 
    ei = ei + e(n)*dt;

    u(n) = kp*e(n) + ki*ei + kd*(e(n)-e_1)/dt;
    if abs(u(n)) > threshold
        u(n) = sign(u(n))*threshold;
    end
    u(n) = u(n) + w(n); % add system noise

    x = A*x + B*u(n);

    u_2 = u_1; u_1 = u(n);
    y_2 = y_1; y_1 = y(n);
    e_1 = e(n);
end
figure
subplot(3,1,1);hold on;grid on
plot(t,yd,LineWidth=2)
plot(t,y, ".-")
legend("yd","y")

subplot(3,1,2);hold on;grid on
plot(t,u,LineWidth=2)
legend("u")

subplot(3,1,3);hold on;grid on
plot(t,e,LineWidth=2)
legend("e",Location="best")

未使用卡尔曼滤波的结果为：

使用了卡尔曼滤波的结果为：

可以看到，卡尔曼滤波的效果非常明显。