卡尔曼状态估计问题和LQR问题的对偶性讨论
背景
我们在讨论连续时间的LQR问题时，引入了一个代价函数，然后利用变分法，求解出了最优控制输入，以及引入了Riccati方程。
\[\begin{aligned} J&=h(x(t_f),t_f)+\int_{t_0}^{t_f}g(x(t),u(t),t)dt\\ -\dot P(t)&=A^TP(t)+P(t)A+C_z^TR_{zz}C_z-P(t)B_uR_{uu}^{-1}B_u^TP(t) \\ \hat u(t)&=-R_{uu}^{-1}B_u^Tp(t)=-R_{uu}^{-1}B_u^TP(t)x(t)\triangleq-K(t)x(t) \end{aligned} \tag{1}\]在卡尔曼状态估计问题中，当我们把离散系统的状态估计问题通过取极限的方式转化成为了连续时间的状态估计问题时，我们也得到了一个形式几乎完全相同的Riccati方程。
考虑一个连续时间的线性系统，其状态方程为：
\[\begin{aligned} \dot x(t)&=Ax(t)+Bu(t)+w(t) \\ y(t)&=Cx(t)+v(t) \\ w(t)&\sim N(0,Q) \\ v(t)&\sim N(0,R) \end{aligned} \tag{2}\]可以到如下状态估计方程：
\[\begin{aligned} -\dot P(t)&=AP(t)+P(t)A^T+Q-P(t)C^TR^{-1}CP(t) \\ \dot{\hat x}(t)&=A\hat x(t)+Bu(t)+K(t)(y(t)-C\hat x(t)) \\ K &\triangleq P(t)C^TR^{-1} \end{aligned} \tag{3}\]我们不难发现这其中的对偶性，如果这两种问题存在对偶性，那么能否通过之前LQR问题中提到的构造一个代价函数并且利用变分法的方式，来求解卡尔曼滤波问题呢？我们下边讨论这个问题。
代价函数
为了不失一般性，我们重新定义一下系统的状态方程和观测方程：
\[\begin{aligned} \dot x(t)&=f(x(t),u(t))+w(t) \\ y(t)&=m(x(t))+v(t) \\ w(t)&\sim N(0,Q) \\ v(t)&\sim N(0,R) \end{aligned} \tag{4}\]我们的目标是求解一个最优的状态估计器，使得状态估计误差的方差最小，我们可以写出如下的代价函数：
\[\begin{aligned} J &= h(x(0)) + \int_{0}^{t_f}g(x(t),w(t),t)dt \\ \end{aligned} \tag{5}\]我们可以观察到，这个代价函数和之前的LQR问题中的代价函数非常相似，但是我们需要注意到，这里的代价函数中并没有出现控制输入$u(t)$，因为在状态估计问题中控制输入量$u(t)$是给定的。相应的，在状态估计问题中，我们需要求解一个最优的$w(t)$，$w(t)$代表着系统的过程误差（噪声）。这样如果结合观测噪声，我们可以按照如下方式去构建代价函数中的$g(x)$：
\[\begin{aligned} g(x(t),w(t),t) &= \frac{1}{2}v(t)^TR^{-1}v(t) + \frac{1}{2}w^T(t)Q^{-1}w(t) \\ &= \frac{1}{2}(y(t)-m(x(t)))^TR^{-1}(y(t)-m(x(t))) + \frac{1}{2}w^T(t)Q^{-1}w(t) \\ \end{aligned} \tag{6}\]由于上边的公式中的$y(t)$也是给定的，因此函数$g$是一个关于$x(t)$、$w(t)$和$t$的函数。
由于在状态估计问题中会给定一个初始状态$\check x_0$和初始方差$\check P_0$，因此初始代价函数$h$可以写成：
\[\begin{aligned} h(x_0) &= \frac{1}{2}(x_0-\check x_0)^T\check P_0^{-1}(x_0-\check x_0) \\ &+ \frac{1}{2}(y_0-m(x_0)^TR^{-1}(y_0 -m(x_0)) \\ \text{sub:} \quad & y_0 \triangleq y(0) \\ & x_0 \triangleq x(0) \\ \text{given:} \quad & \check x_0 \quad \text{and} \quad \check P_0 \\ \end{aligned} \tag{7}\]增广代价函数
参考之前的LQR问题，作为一个约束优化问题，我们可以引入拉格朗日乘子$p(t)$，并且对代价函数进行增广，得到如下的拉格朗日代价：
\[\begin{aligned} J_a &= h(x(0)) + \int_{0}^{t_f}\left[g(x(t),w(t),t) + p(t)^T\{a(x(t),u(t),w(t),t)-\dot x(t)\}\right]dt \\ \dot x(t) &= a(x(t),w(t),t) = f(x(t),u(t)) + w(t)\\ \end{aligned} \tag{8}\]变分法
参考LQR变分法中的讨论，我们可以对上述的代价函数进行变分，得到如下的变分方程：
\[\begin{aligned} \delta J_a &= h_{x_0}\delta x_0 + \int_{0}^{t_f}\left[g_x\delta x + g_w\delta w + (a-\dot x)^T\delta p(t) + p^T(t)(a_x\delta x + a_w\delta w - \delta \dot x)\right]dt \\ &+ [g+p^T(a-\dot x)](t_f)\delta t_f \\ \end{aligned} \tag{9}\]同样的，我们定义哈密顿函数：
\[\begin{aligned} H(x,w,p,t) &= g(x,w,t) + p^Ta(x,w,t) \\ \end{aligned} \tag{10}\]代入哈密顿量，我们得到：
\[\begin{aligned} \delta J_a &= h_{x_0}\delta x_0 + [g+p^T(a-\dot x)](t_f)\delta t_f \\ & + \int_{0}^{t_f}\left[H_x\delta x + H_w\delta w + (a-\dot x)^T\delta p(t) - p^T(t)\delta \dot x\right]dt \\ \end{aligned} \tag{11}\]我们使用分部积分的方式，将上述的积分项进行变换：
\[\begin{aligned} -\int_{0}^{t_f}p^T(t)\delta \dot x dt &= -\int_{0}^{t_f}p^T(t)d\delta x\\ &= -p^T(t)\delta x\bigg\vert_{0}^{t_f} + \int_{0}^{t_f}\dot p^T(t)\delta x dt \\ &= -p^T(t_f)\delta x(t_f) + p^T(0)\delta x(0) + \int_{0}^{t_f}\dot p^T(t)\delta x dt \\ &= -p^T(t_f)(\delta x_{t_f}-\dot x(t_f)\delta t_f) + p^T(0)\delta x_0 + \int_{0}^{t_f}\dot p^T(t)\delta x dt \\ \end{aligned} \tag{12}\]将上述的结果代入到变分方程中，我们得到：
\[\begin{aligned} \delta J_a &= h_{x_0}\delta x_0 + [g+p^T(a-\dot x)](t_f)\delta t_f \\ &+ \int_{0}^{t_f}\left[H_x\delta x + H_w\delta w + (a-\dot x)^T\delta p(t) - p^T(t)\delta \dot x\right]dt \\ &= h_{x_0}\delta x_0 + [g+p^T(a-\dot x)](t_f)\delta t_f \\ &+ \int_{0}^{t_f}\left[H_x\delta x + H_w\delta w + (a-\dot x)^T\delta p(t)\right]dt - \int_{0}^{t_f}p^T(t)\delta \dot x dt \\ &= (h_{x_0}+p_0^T)\delta x_0 + [g+p^Ta](t_f)\delta t_f - p^T(t_f)\delta x_f \\ &+ \int_{0}^{t_f}\left[(H_x+\dot p^T)\delta x + H_w\delta w + (a-\dot x)^T\delta p(t)\right]dt \\ \end{aligned} \tag{13}\]因为在状态估计问题中，$\delta t_f$可以认为是固定的，因此$\delta t_f =0$，由此，我们可以得到如下关系：
\[\begin{aligned} h_{x_0} + p_0^T &= 0 \\ p_f &= 0 \\ H_w &= 0 \\ H_x+\dot p^T&= 0 \\ a-\dot x &= 0 \\ \end{aligned} \tag{14}\]线性系统
我们考虑一个线性系统，其状态方程为：
\[\begin{aligned} \dot x(t)&=Ax(t)+Bu(t)+w(t) \\ y(t)&=Cx(t)+v(t) \\ w(t)&\sim N(0,Q) \\ v(t)&\sim N(0,R) \end{aligned} \tag{15}\]由此我们可得：
\[\begin{aligned} a &= Ax + Bu + w \\ m &= Cx \\ g &= \frac{1}{2}(y-Cx)^TR^{-1}(y-Cx) + \frac{1}{2}w^TQ^{-1}w \\ h(x_0) &= \frac{1}{2}(x_0-\check x_0)^T\check P_0^{-1}(x_0-\check x_0) + \frac{1}{2}(y_0-Cx_0)^TR^{-1}(y_0 -Cx_0) \\ H &= g+p^Ta = \frac{1}{2}(y-Cx)^TR^{-1}(y-Cx) + \frac{1}{2}w^TQ^{-1}w + p^T(Ax+Bu+w) \\ \end{aligned} \tag{16}\]导数：
\[\begin{aligned} g_x &= -(y-Cx)^TR^{-1}C \\ g_w &= w^TQ^{-1} \\ H_x &= g_x + p^TA \\ &= -(y-Cx)^TR^{-1}C + p^TA \\ H_w &= g_w + p^T \\ &= w^TQ^{-1} + p^T \\ h_{x_0} &= \check P_0^{-1}(x_0-\check x_0) - C^TR^{-1}(y_0-Cx_0) \\ &= (\check P_0^{-1} + C^TR^{-1}C)x_0 - (\check P_0^{-1}\check x_0 + C^TR^{-1}y_0) \\ \end{aligned} \tag{17}\]因此根据公式(14)我们可以得到如下关系：
\[\begin{aligned} p_0 &= -h_{x_0}^T \\ &= -(\check P_0^{-1} + C^TR^{-1}C)x_0 + (\check P_0^{-1}\check x_0 + C^TR^{-1}y_0) \\ p_f &= 0 \\ w &= -Qp \\ \dot p &= -H_x^T \\ &= -A^Tp + C^TR^{-1}(y-Cx) \\ &= -A^Tp - C^TR^{-1}Cx + C^TR^{-1}y\\ \dot x &= a \\ &= Ax + Bu + w \\ \end{aligned} \tag{18}\]代入上述的线性系统，我们可以得到如下的状态估计方程：
\[\begin{aligned} \begin{bmatrix} \dot x \\ \dot p \end{bmatrix} &= \begin{bmatrix} A & -Q \\ -C^TR^{-1}C & -A^T \end{bmatrix} \begin{bmatrix} x \\ p \end{bmatrix} + \begin{bmatrix} Bu \\ C^TR^{-1}y \end{bmatrix} \\ \end{aligned} \tag{19}\]到这里，我们推导出了一个和LQR问题中类似的 协状态（costate）转移方程 。
我们进行变量替换，将上述的状态估计方程变成如下形式：
\[\begin{aligned} \begin{bmatrix} \dot x^* \\ \dot p^* \end{bmatrix} &= \begin{bmatrix} A & -Q \\ -C^TR^{-1}C & -A^T \end{bmatrix} \begin{bmatrix} x^* \\ p^* \end{bmatrix} \end{aligned} \tag{19}\]Riccati方程
根据变分法LQR中的推导，具有如下关系：
\[\begin{aligned} p^*(t) &= -P(t)^{-1}x^*(t) \\ x^*(t) &= -P(t)p^*(t) \\ \end{aligned} \tag{20}\]两边同时求导：
\[\begin{aligned} \dot x^*(t) &= -\dot P(t)p^*(t) - P(t)\dot p^*(t) \\ \dot P(t)p^*(t) &= -\dot x^*(t) - P(t)\dot p^*(t) \\ &= -Ax^*+Qp^* + PC^TR^{-1}Cx^* + PA^T \\ &= (AP + PA^T + Q - PC^TR^{-1}CP)p^*(t) \\ \end{aligned} \tag{21}\]于是我们得到：
\[\begin{aligned} \dot P &= AP + PA^T + Q - PC^TR^{-1}CP \\ \end{aligned} \tag{22}\]根据公式（18），我们可以得到初始的$P_0$：
\[\begin{aligned} P_0 &= (\check P_0^{-1} + C^TR^{-1}C)^{-1} \\ \end{aligned} \tag{23}\]卡尔曼增益K
根据公式（19）和（20），我们不难得到如下关系：
\[\begin{aligned} p(t) &= -P^{-1}(t)x(t) + s(t)\\ &\triangleq\ -P^{-1}(t)(x(t) - \bar x(t))\\ \end{aligned} \tag{24}\]因此我们可以得到：
\[\begin{aligned} s(t) &= P^{-1}(t)\bar x(t) \\ \end{aligned} \tag{25}\]不难发现我们需要求解的对于状态的最优估计$\hat x$其实就是$x$的最终状态$x_f$，又由于公式（18）中的边界条件$p_f = 0$，以及公式（24），我们可以得到：
\[\begin{aligned} \hat x = x_f &= \bar x_f \\ \end{aligned} \tag{26}\]有因为公式（25），我们只有得到$s(t)$，就可以得到$\bar x(t)$，进而得到$\hat x$。下面我们考虑如何求解$s(t)$。
将公式（24）两侧同时求导数：
\[\begin{aligned} \dot p = -\dot{P^{-1}}x - P^{-1}\dot x + \dot s \\ \end{aligned} \tag{27}\]代入公式（19）：
\[\begin{aligned} \dot p &= -\dot{P^{-1}}x - P^{-1}\dot x + \dot s \\ -C^TR^{-1}Cx - A^Tp + C^TR^{-1}y &= -\dot{P^{-1}}x - P^{-1}(Ax - Qp + Bu) + \dot s \\ -C^TR^{-1}Cx - A^T(-P^{-1}x + s) + C^TR^{-1}y &= -\dot{P^{-1}}x - P^{-1}(Ax + Bu) + P^{-1}Q(-P^{-1}x + s)+ \dot s \\ (\dot{P^{-1}}-C^TR^{-1}C+A^TP^{-1} + P^{-1}A+P^{-1}QP^{-1})x &= A^Ts - C^TR^{-1}y - P^{-1}Bu + P^{-1}Qs + \dot s \\ \end{aligned} \tag{28}\]根据公式（22），我们可以得到：
\[\begin{aligned} \dot{P^{-1}} &= -P^{-1}\dot P P^{-1} \\ &= -P^{-1}(AP + PA^T + Q - PC^TR^{-1}CP)P^{-1} \\ &= -A^TP^{-1} - P^{-1}A - P^{-1}QP^{-1} + C^TR^{-1}C \\ \end{aligned} \tag{29}\]因此我们可以得到：
\[\begin{aligned} A^Ts - C^TR^{-1}y - P^{-1}Bu + P^{-1}Qs + \dot s = 0 \\ \end{aligned} \tag{30}\]对公式（25）两侧同时求导：
\[\begin{aligned} \dot s &= P^{-1}\dot{\bar x} + \dot{P^{-1}}\bar x \\ \end{aligned} \tag{31}\]将公式（25）和（31）代入公式（30）：
\[\begin{aligned} A^{T}P^{-1}\bar x - C^{T}R^{-1}y - P^{-1}Bu + P^{-1}QP^{-1}\bar x +P^{-1}\dot{\bar x} + \dot{P^{-1}}\bar x &= 0 \\ (\dot{P^{-1}} - C^TR^{-1}C + A^{T}P^{-1} + P^{-1}A + P^{-1}QP^{-1})\bar x - C^{T}R^{-1}y + C^TR^{-1}C\bar x & - P^{-1}A\bar x - P^{-1}Bu + P^{-1}\dot{\bar x} = 0 \\ \dot{\bar x} = A\bar x + Bu + PC^TR^{-1}(y - C\bar x) \\ \end{aligned} \tag{32}\]根据公式（26），我们可以最终得到卡尔曼增益$K$，以及状态估计方程：
\[\begin{aligned} K &= PC^TR^{-1} \\ \dot{\hat x} &= A\hat x + Bu + K(y - C\hat x) \\ \end{aligned} \tag{33}\]结论
卡尔曼状态估计问题和LQR问题都可以通过变分法进行推导，两者的推导过程非常相似，有很强的对偶性。